This commit is contained in:
2025-09-12 15:20:28 +08:00
commit 3257a14c32
449 changed files with 388780 additions and 0 deletions

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,518 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
#always requires a commander (in order to do anything interesting)
#does not create any jobs
include_once processors/ripr_agents/common/common_platform_script.txt
include_once processors/ripr_agents/aiai/behavior_bid_on_jobs.txt
include_once processors/ripr_agents/aiai/behavior_cap-route.txt
include_once processors/ripr_agents/aiai/behavior_default-flight.txt
include_once processors/ripr_agents/aiai/behavior_engage-target.txt
include_once processors/ripr_agents/aiai/behavior_escort.txt
include_once processors/ripr_agents/aiai/behavior_evade.txt
include_once processors/ripr_agents/aiai/behavior_go_home.txt
include_once processors/ripr_agents/aiai/behavior_guide_weapons.txt
include_once processors/ripr_agents/aiai/behavior_pincer.txt
include_once processors/ripr_agents/aiai/behavior_pincer_fsm.txt
include_once processors/ripr_agents/aiai/behavior_pursue-point.txt
include_once processors/ripr_agents/aiai/behavior_pursue-target.txt
include_once processors/ripr_agents/aiai/behavior_radar-control.txt
#include_once processors/ripr_agents/aiai/behavior_sar_job.txt
#include_once processors/ripr_agents/aiai/behavior_weapon-uplink.txt
processor AIAI-thinker WSF_RIPR_PROCESSOR
update_interval 1.0 sec
script_debug_writes off
script_variables
// agent constants
double cMIN_JOB_BID = -MATH.DOUBLE_MAX();
//**********************************************************************//
//** bidding parameters **//
//** slant range and closing speed are the most important **//
//** bids are now in units of distance (meters) **//
//** all other bid contributions are converted with their weight **//
//** range will dominate the bids **//
//**********************************************************************//
double cMAX_SLANT_RANGE = 1000000.0; #over 539 nm away, target ranges beyond this are considered unfavorable
double cWEIGHT_SLANT_RANGE_TO = 1.0;
#double cWEIGHT_SLANT_RANGE_TO = -4.0; #-3.0 (legacy value)
double cMIN_CLOSING_SPEED = -1050.0; #threats running away (negative closing) faster are considered unfavorable
double cWEIGHT_CLOSING_SPEED = 1.0; #scale for how closing speed translates to distance
double cWEIGHT_FUEL = 0.0; #try a value of 2.0 if you care about fuel
double cWEIGHT_MY_WEAPONS_ACTIVE = 0.0; #changes bid if your own weapons are active on target
double cWEIGHT_PEERS_WEAPONS_ACTIVE = 0.0; #changes bid if your peers weapons are active on target
double cWEIGHT_THREAT_WEAPONS_ENVELOPE = 0.0; #uses the global "mWeaponsEnvelope" array, try value of 10000 if you care about that
//careful with these two parameters, they are stateful
double cWEIGHT_CURRENT_TARGET = 0.0; #changes bid if you are currently targeting the threat
#double cWEIGHT_CURRENT_TARGET = 10.0; #cap route jobs
#double cWEIGHT_CURRENT_TARGET = 1.0; #weapon uplink jobs
double cWEIGHT_OTHERS_TARGETING = 0.0; #changes bid if peers are currently targeting the threat
//**********************************************************************//
//** control / mode of operation parameters **//
//**********************************************************************//
bool mFilterJobsOnWeapons = true; # ignore pursue-target jobs that we cant shoot a weapon at
bool mFilterJobsOnCategory = false; # ignore pursue-target jobs that are for platform of unknown category
bool mFilterJobsOnFuel = false; # ignore pursue-target jobs that we dont have the fuel to reach
bool mFilterJobsOnZone = false; # ignore pursue-target jobs that are for platforms outsize of zone "mZoneName"
string mZoneName = "";
WsfZone mFezZone;
string mRequiredWeaponForCap = "";
bool mAllowCoopEngage = true;
string mUplinkSensorName = "geo_sensor"; //name of sensor object
double cDEFAULT_ALTITUDE = 9144; // ~30,000 feet
// the interceptor uses these threat priority pairs to determine who is the most important threat of concern
Map<string, double> ThreatTypePriority = Map<string, double>();
ThreatTypePriority["unknown"] = 0.0;
ThreatTypePriority["uav"] = 2.0;
ThreatTypePriority["sam"] = 4.0;
ThreatTypePriority["ship"] = 4.0;
ThreatTypePriority["awacs"] = -500.0;
ThreatTypePriority["bomber"] = 2.0;
ThreatTypePriority["jammer"] = 0.0;
ThreatTypePriority["fighter"] = 10.0;
ThreatTypePriority["missile"] = 9.0;
ThreatTypePriority["missile_fast"] = 9.0; // try new category for cmd
Array<Map<string, Object>> mWeaponArray = Array<Map<string, Object>>();
////EXAMPLE:
//mWeaponArray[0] = Map<string, Object>();
//mWeaponArray[0].Set("name", "blue_lr_a2a_rf_missile");
//mWeaponArray[0].Set("weapon", PLATFORM.Weapon("blue_lr_a2a_rf_missile"));
//mWeaponArray[0].Set("rangeMin", 1852/2); // only used if the weapon does NOT have a launch computer
//mWeaponArray[0].Set("rangeMax", 60*1852); // only used if the weapon does NOT have a launch computer
//mWeaponArray[0].Set("onlyUseInRange", 1); // only considered if the target is already in range
//mWeaponArray[0].Set("numActiveMax", 1); // how many weapons of this type can be in play simultaneously
//mWeaponArray[0].Set("AIR", 1); // DOMAIN CAPABLE, yes, this weapon can hit air (default true)
//mWeaponArray[0].Set("LAND", 0); // DOMAIN CAPABLE, no, this weapon can NOT hit land (default false)
double mEngagementAggressiveness = 0.4; // value in range [0, 1]. 1 is suicidal, 0 is very cautious.
// used by behavior_in_danger, behavior_evade, & behavior_disengage.
end_script_variables
behavior_tree
behavior_node bid_on_jobs
selector
behavior_node evade
behavior_node go_home
behavior_node escort
behavior_node cap-route
behavior_node pincer_fsm
sequence
selector
behavior_node pursue-target
#behavior_node sar_job
behavior_node pursue-point
behavior_node default-flight
end_selector
behavior_node guide_weapons
end_sequence
end_selector
behavior_node radar-control
behavior_node engage-target
# behavior_node weapon-uplink
end_behavior_tree
###########################################################################
## utility script methods
###########################################################################
script bool HaveWeaponsForThreat(WsfTrack track)
#extern bool IsWeaponDomainCapable(WsfTrack, Map<string, Object>);
#extern Array<Map<string, Object>> mWeaponArray;
foreach (Map<string, Object> curWeapon in mWeaponArray)
{
WsfWeapon weapon = (WsfWeapon)curWeapon["weapon"];
if (weapon.QuantityRemaining() > 0 &&
IsWeaponDomainCapable(track,curWeapon))
{
if (curWeapon.Exists("onlyUseInRange") &&
(int)curWeapon["onlyUseInRange"] == 1 &&
PLATFORM.SlantRangeTo(track) > (double)curWeapon["rangeMax"] )
{
continue;
}
return true;
}
}
return false;
end_script
###########################################################################
## this runs when the processor initializes (when the platform does)
###########################################################################
# on_initialize
# writeln_d(PLATFORM.Name(), " executing on initialze!");
# end_on_initialize
###########################################################################
## deprecated, plz use query_bid_type <job-name> script blocks now
###########################################################################
#query_bid
# return cMIN_JOB_BID;
#end_query_bid
query_bid_type pursue-target
#extern Map<string, double> ThreatTypePriority;
#extern WsfTrack GetTrackByName (WsfPlatform, string);
#extern bool HasEnoughFuelToTravel (WsfPlatform,double);
#extern string DetermineTrackCategory (WsfTrack);
#extern bool TestTrackCategory (WsfTrack, string);
#extern double GetWeaponsEnvelope (WsfPlatform);
#extern double GetWeaponRangeMax(WsfPlatform aPlatform, Array<Map<string, Object>> aWeaponArray);
#extern Array<Map<string, Object>> mWeaponArray;
if (!JOB.IsValid())
{
writeln_d("query_bid_type pursue-target: JOB not valid");
return cMIN_JOB_BID;
}
string targetTrackName = (string)JOB.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetTrackName);
if (!targetTrack.IsValid())
{
writeln_d("!!! No track for JOB: ", JOB.Name(), ", ", JOB.GetDescription(), ", ", targetTrackName);
return cMIN_JOB_BID;
}
double slantRangeTo = PLATFORM.SlantRangeTo(targetTrack);
double closingSpeed = PLATFORM.ClosingSpeedOf(targetTrack);
double maxWeaponRange = GetWeaponRangeMax(PLATFORM, mWeaponArray);
if (mFilterJobsOnFuel == true)
{
if (!HasEnoughFuelToTravel(PLATFORM,slantRangeTo-maxWeaponRange))
{
writeln_d("!!! Not enough fuel for JOB: ", JOB.Name(), ", ", JOB.GetDescription(), ", ", targetTrackName);
return cMIN_JOB_BID;
}
}
if (mFilterJobsOnCategory == true)
{
if (TestTrackCategory(targetTrack, "unknown"))
{
writeln_d("!!! Target type unknown for current job. ");
return cMIN_JOB_BID;
}
}
if (mFilterJobsOnWeapons == true)
{
//check here if we have any weapons remaining that are capable against target domain
if (!HaveWeaponsForThreat(targetTrack) &&
PROCESSOR.WeaponsActive(targetTrack) <= 0)
{
writeln_d("!!! No domain capable weapons left for target!");
return cMIN_JOB_BID;
}
}
if (mFilterJobsOnZone == true)
{
if (mZoneName != "")
{
mFezZone = PLATFORM.Zone(mZoneName);
if (!mFezZone.IsValid() || !mFezZone.PointIsInside(PLATFORM.Location()))
{
writeln_d("!!! Target outside of given FEZ!");
return cMIN_JOB_BID;
}
}
}
double weight_closing_speed = cWEIGHT_CLOSING_SPEED;
if (targetTrack.LandDomain())
{
weight_closing_speed = 2.0 * cWEIGHT_CLOSING_SPEED;
}
/////////////////////////////////////////////////////////////////////////
// calculate bulk of the bid HERE
double bid = 0.0;
bid += cWEIGHT_SLANT_RANGE_TO * (cMAX_SLANT_RANGE - slantRangeTo);
bid += weight_closing_speed * (-cMIN_CLOSING_SPEED + closingSpeed);
// the bid has its major contributers now
/////////////////////////////////////////////////////////////////////////
//calculate other optional bid contributions here
WsfFuel fuelObj = PLATFORM.Fuel();
if (fuelObj.IsValid())
{
bid = bid + cWEIGHT_FUEL * fuelObj.QuantityRemaining();
}
//contribution if I have an active weapon on the target
bid = bid + cWEIGHT_MY_WEAPONS_ACTIVE * PROCESSOR.WeaponsActive(targetTrack);
//contribution if peers have an active weapon on the target
bid = bid + cWEIGHT_PEERS_WEAPONS_ACTIVE * PROCESSOR.PeersWeaponsActive(targetTrack);
//contribution if I am currently targeting the target
if (PROCESSOR.GetTargetName() == targetTrackName)
{
bid = bid + cWEIGHT_CURRENT_TARGET;
}
//contribution if any peers are targeting the target
WsfRIPRProcessor commander = PROCESSOR.GetRIPRCommanderProcessor();
if (commander.IsValid())
{
bid = bid + cWEIGHT_OTHERS_TARGETING * commander.SubsTargeting(targetTrack, PLATFORM);
}
//contribution bonus if you care about weapon envelopes
if (slantRangeTo < GetWeaponsEnvelope(targetTrack.Target()))
{
bid = bid + cWEIGHT_THREAT_WEAPONS_ENVELOPE;
}
//contribution bonus from threat type (category)
string targetType = DetermineTrackCategory(targetTrack);
if( ThreatTypePriority.Exists( targetType ) )
{
bid = bid + ThreatTypePriority.Get( targetType );
}
writeln_d(PLATFORM.Name(), " bid on target ", targetTrackName, ": ", bid);
return bid;
end_query_bid_type
query_bid_type cap-route
//writeln_d("BEHAVIOR NODE query_bid_type cap-route");
if (!JOB.IsValid())
{
return -MATH.DOUBLE_MAX();
}
double current_bid = 10000.0;
Map<string, Object>tempData = JOB.GetData();
WsfGeoPoint point = (WsfGeoPoint)tempData["location"];
double heading = (double)tempData["heading"];
string routeName = (string)tempData["route name"];
WsfRoute route = WsfRoute.FindGlobal(routeName);
if (!point.IsValid())
{
writeln("!!! Invalid point for current job: ",JOB.Name(), ", ", JOB.GetDescription() );
return cMIN_JOB_BID;
}
if (!route.IsValid())
{
writeln("!!! Invalid route for job to bid on: ",JOB.Name(), ", ", JOB.GetDescription() );
return cMIN_JOB_BID;
}
// check fuel levels
#extern bool HasEnoughFuelToTravelRoute(WsfPlatform,WsfRoute);
if (mFilterJobsOnFuel && !HasEnoughFuelToTravelRoute(PLATFORM,route))
{
writeln_d("!!! Not enough fuel for JOB: ", JOB.Name(), ", ", JOB.GetDescription());
return cMIN_JOB_BID;
}
double bid = 1000000 - PLATFORM.SlantRangeTo(point);
if (mRequiredWeaponForCap != "")
{
double Q = PLATFORM.Weapon(mRequiredWeaponForCap).QuantityRemaining();
if (Q <= 0)
{
return 0.001;
}
bid = bid + 20000 * Q;
}
return bid;
end_query_bid_type
query_bid_type pincer
//writeln_d("BEHAVIOR NODE querybid_type pincer");
if (!JOB.IsValid())
{
return cMIN_JOB_BID;
}
double current_bid = 10000.0;
WsfGeoPoint point = (WsfGeoPoint)JOB.GetData("ZonePoint");
if (!point.IsValid())
{
writeln_d("!!! Invalid point for current job: ",JOB.Name(), ", ", JOB.GetDescription() );
return cMIN_JOB_BID;
}
##extern Map<string, double> ThreatTypePriority;
#extern string DetermineTrackCategory (WsfTrack);
#extern bool TestTrackCategory (WsfTrack, string);
#extern double GetWeaponsEnvelope (WsfPlatform);
double slantRangeTo = PLATFORM.SlantRangeTo(point);
// calculate bulk of the bid now //
double weight_closing_speed = 2.0 * cWEIGHT_CLOSING_SPEED;
double closingSpeed = PLATFORM.Speed() * MATH.Cos(PLATFORM.RelativeBearingTo(point));
current_bid += cWEIGHT_SLANT_RANGE_TO * (cMAX_SLANT_RANGE - slantRangeTo);
current_bid += weight_closing_speed * (-cMIN_CLOSING_SPEED + closingSpeed);
// // // // // // // // // //
WsfFuel fuelObj = PLATFORM.Fuel();
if (fuelObj .IsValid())
{
current_bid = current_bid + cWEIGHT_FUEL * fuelObj.QuantityRemaining();
}
current_bid = current_bid + cWEIGHT_MY_WEAPONS_ACTIVE * PROCESSOR.WeaponsActive();
# if (slantRangeTo < GetWeaponsEnvelope(targetTrack.Target()))
# {
current_bid = current_bid + cWEIGHT_THREAT_WEAPONS_ENVELOPE;
# }
# if( ThreatTypePriority.Exists( targetType ) )
# { # current_bid = current_bid + ThreatTypePriority.Get( targetType ); # }
//lets include these in just so this bid is equally competitive with pursue-target jobs
current_bid = current_bid + cWEIGHT_CURRENT_TARGET;
//current_bid = current_bid + cWEIGHT_PEERS_WEAPONS_ACTIVE * (PROCESSOR.PeersWeaponsActive());
#extern Map<string, double> ThreatTypePriority;
#extern string DetermineTrackCategory(WsfTrack);
#extern WsfTrack GetTrackByName (WsfPlatform, string);
Array<string> targetNames = (Array<string>)JOB.GetData("ZoneThreatNameArray");
foreach(string tgtName in targetNames)
{
WsfTrack tgt = GetTrackByName(PLATFORM, tgtName);
if (tgt.IsValid())
{
string targetType = DetermineTrackCategory(tgt);
if( ThreatTypePriority.Exists( targetType ) )
{
current_bid = current_bid + ThreatTypePriority.Get( targetType );
}
}
}
return current_bid;
end_query_bid_type
query_bid_type pursue-point
writeln_d("BEHAVIOR NODE query_bid_type pursue-point");
if (!JOB.IsValid())
{
return cMIN_JOB_BID;
}
double current_bid = 10000.0;
WsfGeoPoint point = (WsfGeoPoint)JOB.GetData("targetPoint");
if (!point.IsValid())
{
writeln_d("!!! Invalid point for current job: ",JOB.Name(), ", ", JOB.GetDescription() );
return cMIN_JOB_BID;
}
double distToPoint = PLATFORM.SlantRangeTo(point);
// check fuel levels
#extern bool HasEnoughFuelToTravel(WsfPlatform,double);
if (mFilterJobsOnFuel &&
!HasEnoughFuelToTravel(PLATFORM,distToPoint))
{
writeln_d("!!! Not enough fuel for JOB: ", JOB.Name(), ", ", JOB.GetDescription());
return cMIN_JOB_BID;
}
current_bid += cWEIGHT_SLANT_RANGE_TO * (cMAX_SLANT_RANGE - distToPoint);
return current_bid;
end_query_bid_type
query_bid_type weapon_uplink
writeln_d("BEHAVIOR NODE query_bid_type weapon_uplink");
if (!JOB.IsValid())
{
return cMIN_JOB_BID;
}
double current_bid = 10000.0;
//check if I can support this weapon
#extern WsfTrack GetTrackByName(WsfPlatform, string);
string name = (string)JOB.GetData("targetTrackName");
int weaponPlatformIndex = (int)JOB.GetData("weaponPlatformIndex");
int targetPlatformIndex = (int)JOB.GetData("targetPlatformIndex");
WsfTrack track = GetTrackByName(PLATFORM, name);
WsfPlatform weaponPlatform = WsfSimulation.FindPlatform(weaponPlatformIndex);
WsfPlatform targetPlatform = WsfSimulation.FindPlatform(targetPlatformIndex);
WsfLocalTrack t = (WsfLocalTrack)track;
if (!weaponPlatform.IsValid() ||
!targetPlatform.IsValid() ||
!track.IsValid())
{
writeln_d("!!! Invalid weapon or target for job: ", JOB.GetDescription() );
return cMIN_JOB_BID;
}
if (!mAllowCoopEngage &&
!t.ContributorOf(PLATFORM))
{
return cMIN_JOB_BID;
}
if (PLATFORM.WithinFieldOfView(targetPlatform, mUplinkSensorName))
{
double alreadyUplinking = 0.0;
if (PROCESSOR.IsUplinkingTo(weaponPlatform))
{
alreadyUplinking = 1.0;
}
current_bid += cWEIGHT_SLANT_RANGE_TO * (cMAX_SLANT_RANGE - PLATFORM.SlantRangeTo(targetPlatform));
current_bid += cWEIGHT_CLOSING_SPEED * (-cMIN_CLOSING_SPEED + PLATFORM.ClosingSpeedOf(targetPlatform));
current_bid += cWEIGHT_CURRENT_TARGET * alreadyUplinking;
}
else
{
writeln_d("!!! target for uplink job is out of view: ", JOB.GetDescription() );
return cMIN_JOB_BID;
}
//writeln_d(" uplink returning calculated bid: ", current_bid);
return current_bid;
end_query_bid_type
########################################################################
### this script block is executed every update interval
### it runs before the behavior tree
########################################################################
# on_update
# #double startTime = PROCESSOR.WallClockTime();
# writeln_d(PLATFORM.Name(), " executing on update!");
# #double duration = PROCESSOR.WallClockTime() - startTime;
# #writeln_d("AIAI-thinker on update duration = ", duration);
# end_on_update
end_processor

View File

@@ -0,0 +1,68 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior anti-radiation_shot
script_variables
bool tempVariable;
end_script_variables
precondition
writeln("precondition anti-radiation_shot");
#extern Array<string> ttrs;
foreach(string ttr in ttrs)
{
if (PLATFORM.AuxDataBool(ttr)) #is this guy painting us with his radar?
{
return true;
}
}
return false;
end_precondition
execute
writeln("executing anti-radiation_shot.");
PLATFORM.Comment("anti-radiation_shot");
########################################################################
### shoot on any target tracking radards that are painting us
########################################################################
#extern Array<string> ttrs;
foreach(string ttr in ttrs)
{
if (PLATFORM.AuxDataBool(ttr)) #shoot at this guy if we can, he's tracking us
{
writeln_d(PLATFORM.Name(), " trying to shoot at ttr: ", ttr);
WsfPlatform ttrPlatform = WsfSimulation.FindPlatform(ttr);
if (ttrPlatform.IsValid()) #take a shot
{
WsfTrack ttrTrack = ttrPlatform.MakeTrack();
ttrTrack.SetAuxData("notlocal", "true");
#extern bool CheckAndFire(WsfTrack);
if (CheckAndFire(ttrTrack))
{
writeln_d(PLATFORM.Name(), " shot at the ttr: ", ttr);
PLATFORM.SetAuxData(ttr, false);
}
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,151 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior avoid_overshoot
script_variables
double mLagPursuitTime = 5.0; #when flying lag pursuit, fly to where the target was this many seconds ago
double mTurnRadius = -1.0;
WsfGeoPoint mTurnPost = WsfGeoPoint();
bool mDrawPost = false;
bool mDrawLag = false;
WsfDraw mDraw = WsfDraw();
double cGRAVITY_ACCEL = 9.80665; # 1 G
double cDEFAULT_ACCEL = (cGRAVITY_ACCEL * 7.5); # m/s^2 ~7.5 Gs
end_script_variables
precondition
writeln("precondition avoid_overshoot");
//check relative position and rates against target
WsfTrack targetTrack = PROCESSOR.GetTarget();
WsfMover mover = PLATFORM.Mover();
if (targetTrack.IsNull() || !targetTrack.IsValid() || mover.IsNull() || !mover.IsValid())
{
writeln("target or mover not valid for executing avoid_overshoot");
return false;
}
//check if target is turning & I'm in pursuit
//check if I'm in danger of overshooting the target
//if inside the turn circle: fly lag
//if outside the turn circle, check closing speed and range: fly to post
WsfPlatform tPlatform = targetTrack.Target();
Vec3 tAccel = tPlatform.AccelerationWCS();
mTurnRadius = tPlatform.VelocityWCS().MagnitudeSquared() / tAccel.Magnitude(); //equation of circular motion
Vec3 tTurnPost = tAccel.Normal();
tTurnPost . Scale(mTurnRadius); //turning post is center of turning circle
tTurnPost = Vec3.Add(tPlatform.Location().LocationWCS(), tTurnPost); //(one radius in the direction of acceleration)
mTurnPost . Set(tTurnPost);
if (!mTurnPost.IsValid() || mTurnPost.Altitude() < 0 || mTurnPost.Altitude() != mTurnPost.Altitude())
{
writeln("mTurnPost is NOT VALID!");
return false;
}
if (PLATFORM.SlantRangeTo(mTurnPost) > mTurnRadius)
{
//we are outside the target's turning circle
double tCloseSpeed = PLATFORM.ClosingSpeedOf(tPlatform);
//don't want to swing it too wide
}
else
{
//we are inside the target's turning circle
return true;
}
return false;
end_precondition
execute
WsfTrack targetTrack = PROCESSOR.GetTarget();
if (targetTrack.IsNull() || !targetTrack.IsValid())
{
writeln("target track not valid for executing avoid_overshoot");
return;
}
writeln("executing avoid_overshoot");
PLATFORM.Comment("avoid_overshoot");
//fly lag &/or climb
double maxLagDist = 0.40 * PLATFORM.SlantRangeTo(targetTrack);
double maxLagTime = maxLagDist / targetTrack.Speed();
double lagTime = mLagPursuitTime;
if (lagTime > maxLagTime)
{
lagTime = maxLagTime;
}
WsfGeoPoint targetLagPos = targetTrack.LocationAtTime(TIME_NOW - lagTime);
//use track's old position (just extrapolated with velocity, backwards)
double tAlt = targetTrack.Altitude();
if (PLATFORM.Altitude() > tAlt)
{
tAlt = PLATFORM.Altitude();
}
PLATFORM.GoToAltitude(tAlt, 100);
PLATFORM.TurnToHeading(PLATFORM.TrueBearingTo(targetLagPos), cDEFAULT_ACCEL);
if (mDrawPost)
{
mDraw.SetLineSize(2);
mDraw.SetColor(0.2, 1.0, 0.2); //greenish
mDraw.SetDuration(PROCESSOR.UpdateInterval());
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(mTurnPost);
mDraw.End();
}
if (mDrawLag)
{
mDraw.SetLineSize(2);
mDraw.SetColor(0.8, 1.0, 0.2); //yellow-ish
mDraw.SetDuration(PROCESSOR.UpdateInterval());
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(targetLagPos);
mDraw.End();
}
double tSpeed = targetTrack.Speed();
//double percentSpeed = 1.0 + (PLATFORM.SlantRangeTo(mTurnPost) / (mTurnRadius * 100)); //vary speed based on distance from from post
double percentSpeed = 0.95; //match the targets speed for now
//writeln("tSpeed=", tSpeed, ", mTurnPost=", mTurnPost.ToString(), ", SlantRangeTo(mTurnPost)=", PLATFORM.SlantRangeTo(mTurnPost), ", mTurnRadius=", mTurnRadius, ", percentSpeed=", percentSpeed);
PLATFORM.GoToSpeed(tSpeed*percentSpeed, cDEFAULT_ACCEL, true);
//use out of plan manuevers if necessary to slow down & preserve energy (climb)
###PLATFORM.GoToAltitude(mBiggestThreat.Altitude(), cDEFAULT_ACCEL, true);
end_execute
end_behavior

View File

@@ -0,0 +1,122 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior bid_on_jobs
script_debug_writes off
script_variables
bool mBidOnOwnJobs = false;
end_script_variables
precondition
writeln_d(PLATFORM.Name(), " precondition bid_on_jobs, T=", TIME_NOW);
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
if (mBidOnOwnJobs == false)
{
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
if (!commander.IsValid())
{
writeln_d("does not have a valid commander");
return Failure("does not have a valid commander");
}
else if (commander.GetJobs().Size() <= 0)
{
writeln_d("commander does not have any jobs");
return Failure("commander does not have any jobs");
}
else
{
return true;
}
}
else
{
if (((WsfRIPRProcessor)PROCESSOR).GetJobs().Size() <= 0)
{
writeln_d("agent does not have any jobs on its own job board");
return Failure("agent does not have any jobs on its own job board");
}
else
{
return true;
}
}
end_precondition
#on_init
#end_on_init
execute
writeln_d(PLATFORM.Name(), " executing bid_on_jobs, T=", TIME_NOW);
Array<WsfRIPRJob> jobs;
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
if (mBidOnOwnJobs == false)
{
if (commander.IsBidWindowOpen())
{
jobs = commander.GetJobs();
}
else
{
writeln_d("commanders bidding window not open");
return;
}
}
else
{
if (((WsfRIPRProcessor)PROCESSOR).IsBidWindowOpen())
{
jobs = ((WsfRIPRProcessor)PROCESSOR).GetJobs();
}
else
{
writeln_d("my bidding window not open");
return;
}
}
foreach (WsfRIPRJob job in jobs)
{
double bid = ((WsfRIPRProcessor)PROCESSOR).QueryBid(job);
if ((bid < 0) && (job.GetProgress(((WsfRIPRProcessor)PROCESSOR)) < 1))
{
writeln_d("bid_on_jobs: job.UnbidJob() for job: ", job.GetDescription());
job.UnbidJob( ((WsfRIPRProcessor)PROCESSOR) );
}
else
{
job.BidJob(((WsfRIPRProcessor)PROCESSOR), bid);
writeln_d(PLATFORM.Name(), " bid for ", job.GetDescription(), " = ", bid, ", priority = ", job.GetPriority());
for (int channel = 1; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
{
double scaledBid = bid * 0.25;
if (bid < 0.0)
{
scaledBid = bid * 2.5;
}
job.BidJob( ((WsfRIPRProcessor)PROCESSOR), channel, scaledBid, 0.0 );
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,167 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior cap-route
script_debug_writes off
script_variables
bool mDrawRoute = true;
WsfDraw mDraw = WsfDraw();
string mOldRouteStr = "no route";
bool mNewRoute = false;
WsfGeoPoint mPoint;
double mHeading;
string mRouteName;
WsfRoute mRoute;
WsfRIPRJob mCurrentJob = null;
end_script_variables
precondition
writeln_d("precondition cap-route");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
string anOldRouteStr = mOldRouteStr;
mOldRouteStr = "no route";
mNewRoute = false;
#extern WsfTrack GetTrackByName (WsfPlatform, string);
#extern bool TestTrackCategory (WsfTrack, string);
if (((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid() &&
((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsJobWindowOpen())
{
mCurrentJob = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
}
if (mCurrentJob.IsNull() ||
!mCurrentJob.IsValid() ||
mCurrentJob.Name() != "cap-route")
{
writeln_d("cap-route -> job not a valid cap-route job");
return Failure("job is not a valid cap-route job");
}
mPoint = (WsfGeoPoint)mCurrentJob.GetData("location");
mHeading = (double)mCurrentJob.GetData("heading");
mRouteName = (string)mCurrentJob.GetData("route name");
WsfRoute route = WsfRoute.FindGlobal(mRouteName);
if (!mPoint.IsValid())
{
writeln("!!! Invalid point for cap-route job: ", mCurrentJob.GetDescription() );
return Failure("cap-route job does not have valid point");
}
if (!route.IsValid())
{
writeln("!!! Invalid route for cap-route job: ", mCurrentJob.GetDescription() );
return Failure("cap-route job does not have route named");
}
mHeading = MATH.NormalizeAngleMinus180_180(mHeading);
########################################################################
### print output / comments for any tartet change
########################################################################
mOldRouteStr = "Job: " + ", " + mCurrentJob.GetDescription();
writeln_d(" - ", mOldRouteStr);
if (mOldRouteStr != anOldRouteStr)
{
PLATFORM.Comment(mOldRouteStr);
mNewRoute = true;
mRoute = WsfRoute.CopyGlobal(mRouteName);
mRoute.Transform(mPoint.Latitude(), mPoint.Longitude(), mHeading);
}
mDraw.SetDuration(((WsfRIPRProcessor)PROCESSOR).UpdateInterval());
return true;
end_precondition
#on_init
#end_on_init
execute
writeln_d(PLATFORM.Name(), " executing cap-route, T=", TIME_NOW);
//PLATFORM.Comment("cap-route");
bool onCap = false;
# WsfRoute myRoute = PLATFORM.Route();
# if(myRoute.IsValid() && myRoute.Name() == "cap_route")
# {
# int routeIndex = PLATFORM.RoutePointIndex();
# if (routeIndex < 0 || routeIndex >= myRoute.Size())
# {
# routeIndex = 0;
# }
#
# double distThreshold = 4.0*185.2; ## 4/10th nm
# if (myRoute.Waypoint(routeIndex).Location().GroundRangeTo(PLATFORM.Location()) < distThreshold)
# {
# routeIndex = routeIndex + 1;
# if (routeIndex >= myRoute.Size())
# {
# routeIndex = 0;
# }
# }
# //onCap = PLATFORM.FollowRoute(myRoute, routeIndex);
#
# WsfWaypoint wpt = myRoute.Waypoint(routeIndex);
# #extern bool FlyTarget(WsfPlatform, WsfGeoPoint, double);
# onCap = FlyTarget(PLATFORM, wpt.Location(), wpt.Speed());
# }
onCap = ( ! PLATFORM.Mover().IsExtrapolating() );
if (!onCap || mNewRoute)
{
WsfRoute capRoute = WsfRoute.Create("cap_route");
WsfRoute givenRoute = WsfRoute.CopyGlobal(mRouteName);
givenRoute.Transform(mPoint.Latitude(), mPoint.Longitude(), mHeading);
capRoute.Append(givenRoute);
PLATFORM.FollowRoute(capRoute);
}
if (mDrawRoute == true)
{
WsfRoute currRoute = PLATFORM.Route();
if (currRoute.IsValid())
{
mDraw.SetLayer("behavior_cap-route");
mDraw.Erase(PLATFORM.Name());
mDraw.SetId(PLATFORM.Name());
mDraw.SetColor(1,0,1);
mDraw.SetLineSize(1);
mDraw.SetLineStyle("dashed");
mDraw.BeginPolyline();
for (int i=0; i<currRoute.Size(); i=i+1)
mDraw.Vertex(currRoute.Waypoint(i).Location());
mDraw.End();
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,160 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior debug_job_board
script_variables
Array<WsfRIPRProcessor> mSubs = Array<WsfRIPRProcessor>();
Array<WsfRIPRJob> mJobs = Array<WsfRIPRJob>();
bool mHideMinBids = true;
double mMinBid = -MATH.DOUBLE_MAX();
end_script_variables
on_init
end_on_init
precondition
writeln_d("precondition debug_job_board");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
mSubs = ((WsfRIPRProcessor)PROCESSOR).GetRIPRSubordinateProcessors();
mSubs.PushBack(((WsfRIPRProcessor)PROCESSOR));
mJobs = ((WsfRIPRProcessor)PROCESSOR).GetJobs();
if( (mSubs.Size() > 0) && (mJobs.Size() > 0))
{
return true;
}
return false;
end_precondition
execute
writeln_d("executing debug_job_board");
writeln("Job Board for ", PLATFORM.Name(), " at Time = ", TIME_NOW);
#write top row (bidder names)
string topRow = " job name, priority, ";
foreach (WsfRIPRProcessor sub in mSubs)
{
string subName = sub.Platform().Name();
while (subName.Length() < 16)
{
subName = " " + subName;
}
if (subName.Length() > 16)
{
subName = subName.Substring(-16);
}
topRow = topRow + subName + ", "; #each column is 18 spaces wide
}
###topRow = topRow.Substring(0,-2); #clip off the last comma & space
topRow = topRow + " winner";
string lines = "-";
while (lines.Length() < topRow.Length())
{
lines = lines + "-";
}
writeln(lines);
writeln(topRow);
foreach (WsfRIPRJob job in mJobs)
{
string jobDesc = job.GetDescription();
while (jobDesc.Length() < 32)
{
jobDesc = " " + jobDesc;
}
if (jobDesc.Length() > 32)
{
jobDesc = jobDesc.Substring(-32);
}
string row = jobDesc + " "; #this should be 33 spaces, at this point
string pString = (string)job.Priority();
while( pString.Contains(".") && pString.Substring(-1)=="0") #take off all trailing zeros
{
pString = pString.Substring(0,-1);
}
if (pString.Length() > 16)
{
pString = pString.Substring(0,15);
}
pString = pString + ", ";
while (pString.Length() < 18)
{
pString = " " + pString;
}
row = row + pString;
foreach (WsfRIPRProcessor sub in mSubs)
{
double bidValue = job.GetBid(sub);
string bidValStr = (string)bidValue;
if (mHideMinBids == true && bidValue <= mMinBid)
{
bidValStr = "-";
}
while( bidValStr.Contains(".") && bidValStr.Substring(-1)=="0") #take off all trailing zeros
{
bidValStr = bidValStr.Substring(0,-1);
}
if (bidValStr.Length() > 16)
{
bidValStr = bidValStr.Substring(0,15);
}
bidValStr = bidValStr + ", ";
while (bidValStr.Length() < 18)
{
bidValStr = " " + bidValStr;
}
row = row + bidValStr;
}
###row = row.Substring(0,-2); #clip off the last comma & space
Array<WsfPlatform> winners = job.Winners();
string winner;
if (winners.Empty())
{
winner = "-";
}
else if (winners.Size() > 1)
{
winner = (string)winners.Size() + " winners";
}
else
{
winner = winners[0].Name();
}
while (winner.Length() < 16)
{
winner = " " + winner;
}
if (winner.Length() > 16)
{
winner = winner.Substring(-16);
}
row = row + winner;
writeln(row);
}
writeln(lines);
writeln("");
end_execute
end_behavior

View File

@@ -0,0 +1,94 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior default-flight
script_debug_writes off
script_variables
bool mDrawRoute = false;
WsfDraw mDraw = WsfDraw();
bool mResumeDefaultRoute = true;
double cDEFAULT_SPEED = 450.0 * MATH.MPS_PER_NMPH();
double cDEFAULT_ACCEL = 7.5 * Earth.ACCEL_OF_GRAVITY(); // 7.5 G (m/s^2)
double cLARGE_UPDATE_INTERVAL = 3.0; // update once every 3 seconds
end_script_variables
script void DrawRoute()
WsfRoute currRoute = PLATFORM.Route();
if (currRoute.IsValid())
{
//PLATFORM.Comment("draw current route");
mDraw.SetLayer("behavior default=flight");
mDraw.Erase(PLATFORM.Name());
mDraw.SetId(PLATFORM.Name());
mDraw.SetColor(0,1,1);
mDraw.SetLineSize(2);
mDraw.SetLineStyle("dash_dot2");
mDraw.BeginPolyline();
for (int i=0; i<currRoute.Size(); i=i+1)
{
mDraw.Vertex(currRoute.Waypoint(i).Location());
}
mDraw.End();
}
end_script
precondition
writeln_d("precondition default-flight");
return true;
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing default-flight, T=", TIME_NOW);
//PLATFORM.Comment("default-flight");
if (mResumeDefaultRoute)
{
bool success = false;
//go at default speed; this gets overwritten if route waypoint has defined a speed
success = PLATFORM.GoToSpeed(cDEFAULT_SPEED, cDEFAULT_ACCEL, true);
//return to route, at the last target route point as re-entry
#success = success && PLATFORM.ReturnToRoute();
#extern bool ReEnterRoute(WsfPlatform);
success = success && ReEnterRoute(PLATFORM);
if (!success)
{
string msg = "ERROR: transition to default flight, could not return to route!";
PLATFORM.Comment(msg);
writeln_d(" T=", TIME_NOW, " ", PLATFORM.Name(), " ", msg);
}
else
{
string msg = write_str("returning to route: ", PLATFORM.Route().Name());
//PLATFORM.Comment(msg);
writeln_d(" T=", TIME_NOW, " ", PLATFORM.Name(), " ", msg);
}
}
else
{
extern double cDEFAULT_ALTITUDE;
#extern bool FlyHold (WsfPlatform, double, double, double);
FlyHold( PLATFORM, PLATFORM.Heading(), cDEFAULT_ALTITUDE, cDEFAULT_SPEED);
}
#PROCESSOR.SetUpdateInterval(cLARGE_UPDATE_INTERVAL);
if (mDrawRoute)
{
DrawRoute();
}
end_execute
end_behavior

View File

@@ -0,0 +1,177 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior disengage
script_debug_writes off
script_variables
double cDIVE_ALTITUDE = 304.8; # 1000 feet
double cDEFAULT_SPEED = 200.0; # m/s
double cMAX_SPEED = 9999999999.99; # m/s
double cGRAVITY_ACCEL = 9.80665; # 1 G
double cDEFAULT_ACCEL = (cGRAVITY_ACCEL * 7.5); # m/s^2 ~7.5 Gs
double cANGLE_TOLERANCE = 45.0; # degrees
bool drawEscape = false;
WsfDraw draw = WsfDraw();
end_script_variables
on_init
draw.SetLineSize(2);
draw.SetColor(1.0, 0.0, 1.0); //purple
end_on_init
script bool IsFlightAsset(WsfPlatform aPlat)
if (aPlat.Mover().IsA_TypeOf("WSF_AIR_MOVER" ) ||
aPlat.Mover().IsA_TypeOf("WSF_6DOF_MOVER") )
{
return true;
}
else if (aPlat.MakeTrack().AirDomain())
{
return true;
}
else if((aPlat.Altitude() > 152.4) && (aPlat.Speed() > 51.44) ) // ~500 feet alt, ~100 knots speed
{
return true;
}
return false;
end_script
script bool IsWeapon(WsfTrack aTrack)
if (aTrack.Target().IsValid() && aTrack.Target().WeaponEngagement().IsValid())
{
return true;
}
return false;
end_script
precondition
writeln("precondition disengage");
//check track list or threat processor for any threats
#extern double mVisualRange;
double totalThreatsWVR = 0.0;
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
if (!t.SideValid() || t.Side() != PLATFORM.Side())
{
if ((!IsWeapon(t)) && (PLATFORM.SlantRangeTo(t) < mVisualRange))
{
writeln_d(" is threat: ", t.TargetName());
totalThreatsWVR = totalThreatsWVR + 1.0;
}
}
}
double assetCount = 0.0;
WsfRIPRProcessor cmdr = PROCESSOR.GetRIPRCommanderProcessor(); # get the flight lead
if (cmdr.IsValid())
{
WsfRIPRProcessor cmdr2 = cmdr.GetRIPRCommanderProcessor(); # check for gci
if (cmdr2.IsValid())
{
foreach (WsfPlatform aifl in cmdr2.Platform().Subordinates())
{
foreach (WsfPlatform aiai in aifl.Subordinates())
{
if (IsFlightAsset(aiai) && PLATFORM.SlantRangeTo(aiai) < mVisualRange)
{
writeln_d(" is asset: ", aiai.Name());
assetCount = assetCount + 1.0;
}
}
}
}
else
{
foreach (WsfPlatform aiai in cmdr.Platform().Subordinates())
{
if (IsFlightAsset(aiai) && PLATFORM.SlantRangeTo(aiai) < mVisualRange)
{
writeln_d(" is asset: ", aiai.Name());
assetCount = assetCount + 1.0;
}
}
}
}
#extern double mEngagementAggressiveness;
double requiredAggressiveness = totalThreatsWVR / (totalThreatsWVR + assetCount);
writeln_d(" required aggress for disengage: ", requiredAggressiveness, " mine: ", mEngagementAggressiveness);
if (mEngagementAggressiveness < requiredAggressiveness)
{
draw.SetDuration(PROCESSOR.UpdateInterval());
return true; //disengage
}
//check if there are any threats that I need to evade
//if there are & I have an energy advantage (altitude &/or speed), then extend the escape & unload
//???
//???
//???
return false;
end_precondition
execute
writeln("executing disengage.");
PLATFORM.Comment("disengage");
#double bestheading = 0.0; //compute this as away from all threats
#PLATFORM.TurnToHeading(bestheading);
//calculate an average heading to incoming platforms weighted by distance
double y = 0;
double x = 0;
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
if (t.IsValid() && (!t.SideValid() || t.Side() != PLATFORM.Side()))
{
double distMod = 1 / PLATFORM.SlantRangeTo(t);
double bearingMod = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(t));
x = x + MATH.Sin(bearingMod) * distMod;
y = y + MATH.Cos(bearingMod) * distMod;
writeln_d(" Incoming ", t.TargetName(), " at distance: ", 1 / distMod, ", bearing: ", bearingMod);
}
}
if (x!=0 || y!=0) //if there is something to run from
{
double evadeHeading = MATH.NormalizeAngle0_360(MATH.ATan2(x, y) - 180);
writeln_d(" x: ", x, ", y: ", y, ", evade at: ", evadeHeading);
#extern bool FlyHold (WsfPlatform, double, double, double);
FlyHold( PLATFORM, evadeHeading, cDIVE_ALTITUDE, cMAX_SPEED);
if (drawEscape == true)
{
WsfGeoPoint pt = PLATFORM.Location();
pt.Extrapolate(evadeHeading, 1852 * 10); //draw line in direction of egress, for 10 nautical miles
draw.BeginLines();
draw.Vertex(PLATFORM.Location());
draw.Vertex(pt);
draw.End();
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,60 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior draw_master_track_list
script_variables
WsfDraw draw = WsfDraw();
end_script_variables
on_init
end_on_init
precondition
writeln_d("precondition draw_master_track_list");
if (PLATFORM.MasterTrackList().Count() <= 0)
{
return false;
}
draw.SetDuration(PROCESSOR.UpdateInterval());
return true;
end_precondition
execute
writeln_d("executing draw_master_track_list");
draw.SetLineSize(2);
draw.BeginLines();
foreach(WsfLocalTrack t in PLATFORM.MasterTrackList())
{
draw.SetColor(1.0, 1.0, 0.0); //yellowish
draw.Vertex(PLATFORM.Location());
draw.Vertex(t.CurrentLocation());
for(int i=0; i<t.RawTrackCount(); i=i+1)
{
WsfTrack x = t.RawTrack(i);
if (x.LocationValid())
{
draw.SetColor(0.0, 1.0, 0.0); //green
draw.Vertex(x.CurrentLocation());
draw.Vertex(t.CurrentLocation());
}
}
draw.SetColor(0.0, 0.0, 1.0); //blue
draw.Vertex(t.CurrentLocation());
draw.Vertex(t.Target().Location());
}
draw.End();
end_execute
end_behavior

View File

@@ -0,0 +1,47 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior draw_target
script_variables
WsfDraw draw = WsfDraw();
end_script_variables
on_init
draw.SetLineSize(2);
draw.SetColor(1.0, 0.0, 1.0); //purple
end_on_init
precondition
writeln("precondition draw_target");
WsfTrack targetTrack = PROCESSOR.GetTarget();
if (targetTrack.IsNull() || !targetTrack.IsValid())
{
writeln_d("target not valid for executing draw_target");
return false;
}
draw.SetDuration(PROCESSOR.UpdateInterval());
return true;
end_precondition
execute
writeln("executing draw_target");
WsfTrack targetTrack = PROCESSOR.GetTarget();
draw.BeginLines();
draw.Vertex(PLATFORM.Location());
draw.Vertex(targetTrack.CurrentLocation());
draw.End();
end_execute
end_behavior

View File

@@ -0,0 +1,629 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
##############################################################################
### assumes exists: #extern string GetWeaponForThreat(WsfPlatform, WsfTrack);
##############################################################################
behavior engage-target
script_debug_writes off
script_variables
//**********************************************************************//
//** platform / agent specific shooting parameters **//
//**********************************************************************//
bool mCoopEngageOne = false;
bool mCoopEngageOneFlightOnly = false;
//specify any targets here that you will always shoot at...
//even disregarding other targets to do so. (use types or names)
Array<string> mMustShootAtCategories = Array<string>();
mMustShootAtCategories[0] = "sam";
mMustShootAtCategories[1] = "FIRE_CONTROL";
double mDegradedFiringAngle = 55.0; //negative if not valid
double mDegradedPercentRange = 0.50; //range constraint if past degraded firing angle
//specify orientation limits for shooting
double mMaxFiringRollAngle = 10.0; //dont shoot if rolled more/less than this
double mMaxFiringPitchAngle = 15.0; //dont shoot if pitched more than this
double mMinFiringPitchAngle = -10.0; //dont shoot if pitched less than this
//**********************************************************************//
//** threat specific shooting parameters **//
//**********************************************************************//
//require different track qualities to fire on different kinds of threats
double DefaultRequiredTrackQuality = 0.49;
Map<string, double> ThreatTypeRequiredTrackQuality = Map<string, double>();
ThreatTypeRequiredTrackQuality["bomber"] = 0.49;
ThreatTypeRequiredTrackQuality["fighter"] = 0.49;
//fire off different salvos at different types of threats
int DefaultAirSalvo = 1;
int DefaultGndSalvo = 1;
Map<string, int> ThreatTypeSalvo = Map<string, int>();
ThreatTypeSalvo["sam"] = 2;
ThreatTypeSalvo["ship"] = 2;
ThreatTypeSalvo["bomber"] = 2;
ThreatTypeSalvo["fighter"] = 1;
ThreatTypeSalvo["FIRE_CONTROL"] = 1;
ThreatTypeSalvo["primary_target"] = 2;
ThreatTypeSalvo["secondary_target"] = 2;
//force a specific weapon for use against given threat type
Map<string, string> ThreatTypeWeapon = Map<string, string>();
//ThreatTypeWeapon["uav"] = "srm";
//ThreatTypeWeapon["fighter"] = "lrm";
//**********************************************************************//
//** weapon + threat specific shooting parameters **//
//**********************************************************************//
//specify an Rmax based on which weapon used and which threat engaged
double DefaultPercentRangeMax = 0.80; // don't launch unless within this percent of Rmax
double DefaultPercentRangeMin = 1.20; // don't launch unless beyond this percent of Rmin
Map<string, Map<string, double>> WeaponThreatRmaxMap = Map<string, Map<string, double>>();
WeaponThreatRmaxMap["base_weapon"] = Map<string, double>();
WeaponThreatRmaxMap["base_weapon"].Set("fighter", 0.80);
//specify max firing angles based on weapon used and threat engaged
double DefaultMaxFiringAngle = 45.0;
Map<string, Map<string, double>> WeaponThreatAngleMap = Map<string, Map<string, double>>();
WeaponThreatAngleMap["base_weapon"] = Map<string, double>();
WeaponThreatAngleMap["base_weapon"].Set("fighter", 45.0);
WeaponThreatAngleMap["base_weapon"].Set("missile", 30.0);
//**********************************************************************//
//********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
//**********************************************************************//
double mInactiveLaunchDelay = 1.0; // wait 1 seconds after your last missile detonated.
// hacky, useful for weapons that dont show active
Map<string,double> mTrackWeaponActiveMap = Map<string,double>();
Map<WsfTrack, double> mRoundsFiredAtMap = Map<WsfTrack, double>(); //useful when weapons don't show as active
end_script_variables
script bool MustShootAt(WsfTrack track)
WsfPlatform plat = WsfSimulation.FindPlatform( track.TargetName() );
if (plat.IsValid())
{
foreach( string aCategory in mMustShootAtCategories )
{
if( plat.CategoryMemberOf( aCategory ) )
{
return true;
}
}
}
return false;
end_script
script int GetSalvoForThreat(WsfTrack track)
#writeln_d("checking salvo size for category: ", category);
#WsfPlatform plat = WsfSimulation.FindPlatform( track.TargetIndex() );
WsfPlatform plat = WsfSimulation.FindPlatform( track.TargetName() );
if (plat.IsValid())
{
foreach( string aCategory : int salvo in ThreatTypeSalvo )
{
if( plat.CategoryMemberOf( aCategory ) )
{
writeln_d("salvo for type ", aCategory, " = ", salvo);
return salvo;
}
}
}
#extern string GetTargetDomain(WsfTrack);
string sTargetDomain = GetTargetDomain(track);
if ( (sTargetDomain == "LAND") || (sTargetDomain == "SURFACE") )
{
return DefaultGndSalvo;
}
return DefaultAirSalvo;
end_script
script double GetRequiredTrackQualityForThreat(WsfTrack threat)
writeln_d("checking required TQ for track: ", threat.TargetName());
WsfPlatform plat = WsfSimulation.FindPlatform( threat.TargetName() );
if (plat.IsValid())
{
foreach( string aCategory : double quality in ThreatTypeRequiredTrackQuality )
{
if( plat.CategoryMemberOf( aCategory ) )
{
writeln_d("TQ for type ", aCategory, " = ", quality);
return quality;
}
}
}
return DefaultRequiredTrackQuality;
end_script
script double GetLaunchPercentRangeMaxOnThreat(string weaponName, WsfTrack threat)
WsfPlatform plat = WsfSimulation.FindPlatform( threat.TargetName() );
if (plat.IsValid())
{
if (WeaponThreatRmaxMap.Exists(weaponName))
{
Map<string, double> categoryRangeMap = WeaponThreatRmaxMap.Get(weaponName);
foreach (string aCategory : double percent in categoryRangeMap)
{
if( plat.CategoryMemberOf( aCategory ) )
{
return percent;
}
}
}
}
return DefaultPercentRangeMax;
end_script
script double GetMaxFiringAngleForWeapon(string weaponName, WsfTrack threat)
WsfPlatform plat = WsfSimulation.FindPlatform( threat.TargetName() );
if (plat.IsValid())
{
if (WeaponThreatAngleMap.Exists(weaponName))
{
Map<string, double> threatAngleMap = WeaponThreatAngleMap.Get(weaponName);
foreach (string aCategory : double angle in threatAngleMap)
{
if( plat.CategoryMemberOf( aCategory ) )
{
return angle;
}
}
}
}
return DefaultMaxFiringAngle;
end_script
script string GetWeaponForThreat(WsfPlatform platform, WsfTrack track)
bool checkName = false;
string name = "";
#extern bool IsWeaponDomainCapable(WsfTrack, Map<string, Object>);
WsfPlatform plat = WsfSimulation.FindPlatform( track.TargetName() );
if (plat.IsValid())
{
foreach( string aCategory : string wpnStr in ThreatTypeWeapon )
{
writeln_d("checking if ", plat.Name(), " is category: ", aCategory);
if( wpnStr.Length()>0 && plat.CategoryMemberOf( aCategory ) )
{
checkName = true;
name = wpnStr;
writeln_d(" Time= ", TIME_NOW, " ", PLATFORM.Name(), " searching only for weapon category: ", name, " (for use against ", plat.Name(), ")");
break;
}
}
}
else
{
writeln_d("platform for target ", track.TargetName()," is not valid!");
}
extern Array<Map<string, Object>> mWeaponArray;
foreach (Map<string, Object> curWeapon in mWeaponArray)
{
string weaponName = (string)curWeapon["name"];
WsfWeapon weapon = (WsfWeapon)curWeapon["weapon"];
writeln_d(" checking weapon ", weaponName, " valid=", weapon.IsValid());
if (checkName && weaponName != name)
{
continue;
}
if (weapon.QuantityRemaining() <= 0)
{
continue;
}
if (!IsWeaponDomainCapable(track,curWeapon))
{
continue;
}
WsfLaunchComputer lcPtr = weapon.LaunchComputer();
if (lcPtr.IsValid() &&
lcPtr.IsA_TypeOf("WSF_AIR_TO_AIR_LAUNCH_COMPUTER"))
{
writeln_d(" using air-to-air launch computer");
// The returned array contains: Rmax, RmaxTOF, Rne, RneTOF, Rmin, RminTOF
// in that order. -1.0 means "not valid".
Array<double> returnedValues = lcPtr.LookupResult(track);
// Now have to consider whether we have enough information to continue with a weapon shot:
double theRmax = returnedValues[0]; //"Rmax";
double theRmaxTOF = returnedValues[1]; //"RmaxTOF";
double theRne = returnedValues[2]; //"Rne";
double theRneTOF = returnedValues[3]; //"RneTOF";
double theRmin = returnedValues[4]; //"Rmin";
double theRminTOF = returnedValues[5]; //"RminTOF";
double range = 0.0;
if (track.RangeValid())
{
range = track.Range(); #is this the range from you to the track, or the range from the sensor to the track?
}
else
{
range = track.GroundRangeTo(PLATFORM);
}
// Check for track range less than Rmin * scaleFactor, if not, return.
// But do not check for min range constraint at all unless we are likely to be needing it.
if (range < 5000)
{
if (theRmin == -1.0)
{
writeln_d(" Engagement did not shoot since Rmin was not valid.");
continue;
#return;
}
double RminConstraint = theRmin * DefaultPercentRangeMin;
if (range < RminConstraint)
{
writeln_d(" Engagement did not shoot since inside the k * Rmin constraint distance.");
writeln_d(" Range versus Rmin constraint = ", range, ", ", RminConstraint);
continue;
#return;
}
}
// Check for track range less than Rne, if so, FORCE a weapon fire.
bool forceWeaponFire = false;
if (range < theRne)
{
writeln_d(" Engagement is forcing a weapon fire due to inside Rne.");
writeln_d(" Range versus Rne constraint = ", range, ", ", theRne);
forceWeaponFire = true;
}
if (forceWeaponFire == false)
{
######################################TRY THIS######################################
WsfPlatform plat = WsfSimulation.FindPlatform( track.TargetName() );
if (plat.IsValid() && plat.CategoryMemberOf("fighter"))
{
#theRmax = theRne;
theRmax = (theRmax + theRne)/2.0; //for highly maneuverable fighter targets
}
####################################END TRY THIS####################################
// Check for track range less than k * Rmax, if not, return.
if (theRmax == -1.0)
{
writeln_d(" Engagement did not shoot since Rmax was not valid.");
continue;
#return;
}
//double RmaxConstraint = theRmax * DefaultPercentRangeMax;
double percentRMax = GetLaunchPercentRangeMaxOnThreat(weaponName, track);
double RmaxConstraint = theRmax * percentRMax;
if (range > RmaxConstraint)
{
writeln_d(" Engagement did not shoot since outside the k * Rmax constraint distance.");
writeln_d(" Range versus Rne constraint = ", range, ", ", theRne);
continue;
#return;
}
}
writeln_d(" Engagement meets constraints for firing a weapon (continue).");
}
else if (lcPtr.IsValid() &&
lcPtr.IsA_TypeOf("WSF_ATG_LAUNCH_COMPUTER"))
{
writeln_d(" using air-to-ground launch computer");
if (lcPtr.CanIntercept(track))
{
//intercept works, this weapon is a candidate
}
else
{
continue;
}
}
else
{
writeln_d(" using script input mWeaponArray array range values");
//check our own ranges & angles --> hacky!!!
#extern double EffectiveRange(WsfPlatform, WsfTrack);
double effectiveRange = EffectiveRange(platform, track);
double absRelativeBearing = MATH.Fabs(PLATFORM.RelativeBearingTo( track ));
double rangeScale = GetLaunchPercentRangeMaxOnThreat(weaponName, track);
if (absRelativeBearing > mDegradedFiringAngle)
{
rangeScale = MATH.Min(mDegradedPercentRange, rangeScale);
}
if ((double)curWeapon["rangeMin"] > effectiveRange)
{
writeln_d(" target too close");
continue;
}
if (absRelativeBearing > GetMaxFiringAngleForWeapon(weaponName, track))
{
writeln_d(" target firing angle too large");
continue;
}
if ((double)curWeapon["rangeMax"] * rangeScale < effectiveRange)
{
writeln_d(" target too far away");
continue;
}
double TOF = 180;
double missileAvgSpeed = 1000;
if (curWeapon.Exists("avgSpeed"))
{
missileAvgSpeed = (double)curWeapon["avgSpeed"];
}
if (curWeapon.Exists("TOF"))
{
TOF = (double)curWeapon["TOF"];
}
double range = PLATFORM.SlantRangeTo(track);
//double closingSpeed = PLATFORM.ClosingSpeedOf(track);
double relBearing = track.RelativeBearingTo(PLATFORM);
if (relBearing > 90.0)
{
if (track.Speed() > missileAvgSpeed)
{
continue;
}
double speedDiff = missileAvgSpeed - track.Speed();
if ((range/speedDiff) > TOF)
{
continue;
}
}
}
return (string)curWeapon["name"];
}
return "";
end_script
#on_init
#end_on_init
precondition
writeln_d("precondition engage-target");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
#extern WsfTrack GetTrackByName(WsfPlatform, string);
double pitch = PLATFORM.Pitch();
if (MATH.Fabs(PLATFORM.Roll()) > mMaxFiringRollAngle ||
pitch > mMaxFiringPitchAngle ||
pitch < mMinFiringPitchAngle)
{
string msgStr = write_str(" ", PLATFORM.Name(), " orientation too far off to fire! (roll or pitch)");
writeln_d(msgStr);
//PLATFORM.Comment(msgStr);
return Failure(msgStr);
}
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
if (commander.IsValid())
{
#check all job channels, not just the main target
for (int channel = 0; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
{
WsfRIPRJob aJob = commander.GetJobFor(((WsfRIPRProcessor)PROCESSOR), channel);
if (aJob.IsValid() &&
aJob.Name() == "pursue-target")
{
string targetName = (string)aJob.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (!targetTrack.IsNull() &&
targetTrack.IsValid())
{
#extern bool TestTrackCategory( WsfTrack, string );
if (!TestTrackCategory(targetTrack, "unknown") &&
targetTrack.BelievedAlive())
{
return true;
}
}
}
}
}
return Failure("no acceptable target to shoot at!");
end_precondition
script bool FireWeapon(WsfTrack targetTrack)
bool launched = false;
writeln_d(" Time= ", TIME_NOW, " Attempting a shot against: ", targetTrack.TargetName(), " Index: ", targetTrack.TargetIndex(), " Type: ", targetTrack.TargetType());
#extern bool TestTrackCategory(WsfTrack, string);
if (!TestTrackCategory(targetTrack, "unknown") &&
targetTrack.BelievedAlive())
{
if ((((WsfRIPRProcessor)PROCESSOR).WeaponsActive(targetTrack) > 0) ||
(((WsfRIPRProcessor)PROCESSOR).PeersWeaponsActive(targetTrack) > 0))
{
mTrackWeaponActiveMap[targetTrack.TargetName()] = TIME_NOW;
writeln_d(" FAIL: Weapons already active against ", targetTrack.TargetName());
return launched;
}
else
{
writeln_d("no weapons active against target ", targetTrack.TargetName());
}
if ((TIME_NOW - mTrackWeaponActiveMap[targetTrack.TargetName()]) < mInactiveLaunchDelay)
{
writeln_d(" FAIL: Waiting to see what last active weapon did, score a kill?");
return launched;
}
// if this is a ttr, it should ignore the check for coop engage one
if (targetTrack.CheckAuxData("notlocal") == false)
{
if (mCoopEngageOne == false)
{
WsfLocalTrack targetLocalTrack = (WsfLocalTrack)targetTrack;
if (targetLocalTrack.IsValid())
{
if(!targetLocalTrack.ContributorOf(PLATFORM) &&
!targetLocalTrack.IsPredefined())
{
writeln_d(" FAIL: Not able to coop engage! ", PLATFORM.Name(), " targeting ",targetTrack.TargetName(), ". NumContributors: ", targetLocalTrack.NumContributors() );
return launched;
}
}
}
else if (mCoopEngageOneFlightOnly == true)
{
//check if this platform needs somebody in his flight to have track on the target
WsfLocalTrack targetLocalTrack = (WsfLocalTrack)targetTrack;
if (targetLocalTrack.IsValid())
{
if (targetLocalTrack.IsPredefined())
{
//its fine, go ahead & shoot
}
else
{
bool OkToShoot = false;
Array<WsfPlatform> peers = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().SubordinatePlatforms();
foreach (WsfPlatform peer in peers)
{
if(targetLocalTrack.ContributorOf(peer))
{
OkToShoot = true;
break;
}
}
if (!OkToShoot)
{
writeln_d(" FAIL: Cant engage tracks not supported by flight group! ", PLATFORM.Name(), " targeting ", targetTrack.TargetName(), ".");
return launched;
}
}
}
}
}
// We can launch once we have a target-quality track.
//TODO probably need something to test for failure to lockon
writeln_d (" targetTrack.TrackQuality == ", targetTrack.TrackQuality());
if (targetTrack.TrackQuality() < GetRequiredTrackQualityForThreat(targetTrack))
{
writeln_d(" FAIL: track quality not good enough to fire on target");
return launched;
}
string selectedWeapon = GetWeaponForThreat(PLATFORM, targetTrack); #checks domain & kinematic capability, & valid quantity remaining
if (selectedWeapon == "")
{
writeln_d(" FAIL: No domain capable weapon within range available!");
return launched;
}
WsfWeapon weaponToLaunch = PLATFORM.Weapon(selectedWeapon);
//writeln_d(" using a conventional weapon!");
#extern Array<Map<string, Object>> mWeaponArray;
#extern int GetWeaponNumberActiveMax(string, Array<Map<string, Object>>);
int maxActiveOfType = GetWeaponNumberActiveMax(selectedWeapon,mWeaponArray);
int curActiveOfType = ((WsfRIPRProcessor)PROCESSOR).WeaponsActiveOfType(weaponToLaunch);
if (MustShootAt(targetTrack))
{
curActiveOfType = maxActiveOfType - 1;
}
if (curActiveOfType >= maxActiveOfType)
{
writeln_d(" FAIL: Max number(", maxActiveOfType,") of ", selectedWeapon, " weapon type are already active! (", curActiveOfType,")");
return launched;
}
int salvoCount = GetSalvoForThreat(targetTrack);
writeln_d(" salvo count for ", targetTrack, " is: ", salvoCount);
#extern bool LaunchWeapon(WsfPlatform, WsfTrack, WsfWeapon, int);
launched = LaunchWeapon(PLATFORM, targetTrack, weaponToLaunch, salvoCount);
writeln_d(" launched == ", launched, ", weapon: ", selectedWeapon);
if (launched == true)
{
double dPreviousRoundsFiredAt = mRoundsFiredAtMap.Get(targetTrack);
mRoundsFiredAtMap.Set(targetTrack,dPreviousRoundsFiredAt + salvoCount);
string msg = "Shot at: " + targetTrack.TargetName();
PLATFORM.Comment(msg);
writeln_d(" SUCCESS: ", PLATFORM.Name(), " ", msg);
}
return launched;
}
return launched;
end_script
execute
writeln_d(PLATFORM.Name(), " executing engage-target, T=", TIME_NOW);
#extern WsfTrack GetTrackByName(WsfPlatform, string);
#check all possible targets on all channels
########################################################################
### fire on any pursue-target jobs we are assigned to
########################################################################
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
for (int channel = 0; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
{
WsfRIPRJob aJob = commander.GetJobFor(((WsfRIPRProcessor)PROCESSOR), channel);
if (aJob.IsValid() &&
aJob.Name() == "pursue-target")
{
string targetName = (string)aJob.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (!targetTrack.IsNull() &&
targetTrack.IsValid())
{
writeln_d(" ", PLATFORM.Name(), " trying to shoot at job track: ", targetName, " at time: ", TIME_NOW);
if(FireWeapon(targetTrack) == false)
{
writeln_d(" ", PLATFORM.Name(), " could NOT fire at track: ", targetTrack.TargetName(), " at time: ", TIME_NOW);
}
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,322 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior escort
script_debug_writes off
script_variables
//**********************************************************************//
//** debugging parameters **//
//**********************************************************************//
bool mDrawEscortData = false;
//**********************************************************************//
//** control / mode of operation parameters **//
//**********************************************************************//
bool mCheckOnlyJobs = true;
bool mCheckMasterTracks = false;
//**********************************************************************//
//** escort parameters (who to escort, and what ranges) **//
//**********************************************************************//
Array<string> mEscortNames = Array<string>();
double mEscortProtectDistance = 100.0 * MATH.M_PER_NM(); #engage threats within this range of the escort package
double mEscortChaseDistance = 15.0 * MATH.M_PER_NM(); #allowed to chase a threat this far out of protect area if it entered
double mWeaponRangeToInclude = 50.0 * MATH.M_PER_NM();
//**********************************************************************//
//** flying parameters, for offset and tightness **//
//**********************************************************************//
double mFormationPositionX = 0; #meters in front of of package
double mFormationPositionY = 0; #meters off right wing of package
double mGoodFormationRatio = 0.15; #percentage of total offset
double mFormationLookAhead = 30.0; #seconds
double mFormationAltitude = -1.0; #used if positive
//**********************************************************************//
//********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
//**********************************************************************//
WsfDraw mDraw = WsfDraw();
bool mLastInPosition = false; #boolean flag, utility, don't change
end_script_variables
### find escorted platform, return it
script WsfPlatform GetEscortPackage()
WsfPlatform escortPlatform;
foreach ( string sEscortName in mEscortNames )
{
escortPlatform = WsfSimulation.FindPlatform(sEscortName);
if (escortPlatform.IsValid() )
{
break;
}
}
return escortPlatform;
end_script
script bool IsAThreatToEscortPackage(WsfTrack aTrack, WsfPlatform aPackage)
double slantRange = aPackage.SlantRangeTo(aTrack);
double currentlyTargeted = 0;
if (((WsfRIPRProcessor)PROCESSOR).GetTargetName() == aTrack.TargetName())
{
currentlyTargeted = 1.0;
}
# determine if the target is a threat to your escort package, check if it lies within the protect radius
# if already engaging the threat, allow a little chase
if (slantRange < (mEscortProtectDistance + mWeaponRangeToInclude + (currentlyTargeted * mEscortChaseDistance)))
{
writeln_d("Target ", aTrack.TargetName()," is a threat to escort package! Stop formation flying, go engage!!!");
return true;
}
return false;
end_script
script void DrawThreat(WsfTrack track)
if (!track.IsValid())
{
return;
}
if (mDrawEscortData == true)
{
mDraw.SetLayer("behavior_escort");
mDraw.SetDuration(PROCESSOR.UpdateInterval());
// draw line to desired formation position
mDraw.SetColor(1, 0, 0);
mDraw.SetLineSize(5);
mDraw.SetLineStyle("dashed");
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(track.CurrentLocation());
mDraw.End();
mDraw.SetLineSize(2);
mDraw.SetLineStyle("solid");
mDraw.SetEllipseMode("line");
mDraw.SetColor(0, 1, 1);
mDraw.BeginEllipse(0,20,20);
mDraw.Vertex(track.CurrentLocation());
mDraw.End();
}
end_script
script bool HaveAPursueTargetJob()
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
if (commander.IsValid())
{
for (int channel = 0; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
{
WsfRIPRJob job = commander.GetJobFor(((WsfRIPRProcessor)PROCESSOR), channel);
if (job.IsValid() && job.Name() == "pursue-target")
{
return true;
}
}
}
return false;
end_script
precondition
writeln_d("precondition escort");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
WsfPlatform escortPlatform = GetEscortPackage();
if (!escortPlatform.IsValid() )
{
return Failure("no listed escort packages found to escort");
}
//check jobs?
//check tracks?
if (mCheckOnlyJobs == true &&
((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid())
{
bool packageInDanger = false;
Array<WsfRIPRJob> jobs = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobs();
Array<WsfTrack> threats = Array<WsfTrack>();
foreach (WsfRIPRJob job in jobs)
{
if (job.Name() == "pursue-target")
{
string name = (string)job.GetData("targetTrackName");
#extern WsfTrack GetTrackByName(WsfPlatform, string);
WsfTrack track = GetTrackByName(PLATFORM, name);
if (track.IsValid())
{
if (IsAThreatToEscortPackage(track, escortPlatform))
{
threats.PushBack(track);
packageInDanger = true;
}
else if (track.AirDomain())
{
#unbid job, so we cant go after anything thats not a threat to our package
for (int channel = 0; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
{
job.UnbidJob( ((WsfRIPRProcessor)PROCESSOR), channel);
}
}
}
}
}
if (packageInDanger == true && HaveAPursueTargetJob())
{
foreach (WsfTrack threat in threats)
{
DrawThreat(threat);
}
return Failure("target is threat to package"); //pre-condition not met, can't just fly in formation
}
}
else if (mCheckMasterTracks == true)
{
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
//check if this track is a threat
if (t.SideValid() &&
t.Side() == PLATFORM.Side())
{
continue;
}
if (IsAThreatToEscortPackage(t, escortPlatform))
{
DrawThreat(t);
return Failure("track is threat to package"); //pre-condition not met, can't just fly in formation
}
}
}
writeln_d("--- agent ", PLATFORM.Name(), " is escorting ", escortPlatform.Name());
return true;
end_precondition
script bool AmInFormationPosition(WsfGeoPoint formationPoint, double radius)
//make it fly to the inner 50% of the acceptable radius on re-entry
if (!mLastInPosition)
{
radius = 0.5 * radius;
}
mLastInPosition = false;
double range = PLATFORM.GroundRangeTo(formationPoint);
if (range <= radius)
{
mLastInPosition = true;
}
return mLastInPosition;
end_script
execute
writeln_d(PLATFORM.Name(), " executing escort, T=", TIME_NOW);
//PLATFORM.Comment("escort");
//make sure escorted platform exists, find it, save it
WsfPlatform escortPlatform = GetEscortPackage();
if (!escortPlatform.IsValid() )
{
writeln_d("--- fighter ", PLATFORM.Name(), " no valid platform to escort!");
return;
}
//check if I am approximately in my formation position
WsfGeoPoint formationPoint = escortPlatform.Location();
if (mFormationAltitude > 0)
{
formationPoint.Set(formationPoint.Latitude(), formationPoint.Longitude(), mFormationAltitude);
}
if (mFormationPositionX > 0)
{
formationPoint.Extrapolate(escortPlatform.Heading(), mFormationPositionX);
}
else
{
formationPoint.Extrapolate(escortPlatform.Heading()+180, -mFormationPositionX);
}
if (mFormationPositionY > 0)
{
formationPoint.Extrapolate(escortPlatform.Heading()+90.0, mFormationPositionY);
}
else
{
formationPoint.Extrapolate(escortPlatform.Heading()-90.0, -mFormationPositionY);
}
if (!formationPoint.IsValid())
{
writeln_d("--- fighter ", PLATFORM.Name(), " invalid formation point from ", escortPlatform.Name());
return;
}
if (mDrawEscortData == true)
{
mDraw.SetLayer("behavior_escort");
mDraw.SetDuration(((WsfRIPRProcessor)PROCESSOR).UpdateInterval());
// draw protection bubble
mDraw.SetColor(0.5, 0.5, 0.5, 0.33); //gray with 0.33 alpha
mDraw.SetEllipseMode("fill");
double R = mEscortProtectDistance + mWeaponRangeToInclude;
mDraw.BeginEllipse(0, R, R);
mDraw.Vertex(escortPlatform.Location());
mDraw.End();
// draw line to desired formation position
mDraw.SetColor(0, 1, 0); //green
mDraw.SetLineSize(4);
mDraw.BeginLines();
mDraw.Vertex(escortPlatform.Location());
mDraw.Vertex(formationPoint);
mDraw.End();
}
double radius = mGoodFormationRatio * escortPlatform.SlantRangeTo(formationPoint);
double formAlt = escortPlatform.Altitude();
double formSpeed = escortPlatform.Speed();
double formYaw = escortPlatform.Heading();
if (formAlt < PLATFORM.Altitude())
{
formAlt = PLATFORM.Altitude();
}
WsfGeoPoint tgt = WsfGeoPoint.Construct(formationPoint.Latitude(), formationPoint.Longitude(), formAlt);
double lookAheadDistance = mFormationLookAhead * escortPlatform.Speed();
tgt.Extrapolate(formYaw, lookAheadDistance);
if (! AmInFormationPosition(formationPoint, radius))
{
//fly towards formation position, extrapolated ahead X seconds, in order to catch up to the point in X seconds
formSpeed = PLATFORM.GroundRangeTo(tgt) / mFormationLookAhead;
}
#extern bool FlyTarget (WsfPlatform, WsfGeoPoint, double);
FlyTarget (PLATFORM, tgt, formSpeed);
end_execute
end_behavior

View File

@@ -0,0 +1,450 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior evade
script_debug_writes off
script_variables
//**********************************************************************//
//** debugging parameters **//
//**********************************************************************//
bool mDrawNearestThreat = false;
bool mDrawEvasionVector = false;
//**********************************************************************//
//** control / mode of operation parameters **//
//**********************************************************************//
double weightPeersForEvade = 0.25; //percentage to scale peers influence for evasion vector
bool mWobbleLeftRight = true;
bool mWobbleUpDown = true;
double cFAST_UPDATE_INTERVAL = 0.25;
double cSLOW_UPDATE_INTERVAL = 2.0;
//**********************************************************************//
//** flying parameters, for for evasive manuevering **//
//**********************************************************************//
double cNORMAL_SPEED = 200.0; // m/s
double cEVADE_SPEED = 1000.0; // m/s (faster than a speeding bullet...M3.0+)
double mAltitudeMin = 1000.0; //meters
double mAltitudeMax = 20000.0; //meters
double mAltitudeToDiveEvade = 1500.0; //distance to dive (meters) (~5000 ft)
double mBankAngleForEvading = 45.0; //banks left & right, back & forth, at this angle, while evading
double mMaxClimbRate = 500.0; // meters/second
double mVerticalAccel = 1.5 * Earth.ACCEL_OF_GRAVITY(); // meters/second^2
//**********************************************************************//
//********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
//**********************************************************************//
double mLastTime = 0.0;
double mClimbRate = 0.0;
Array<WsfPlatform> mIncoming = Array<WsfPlatform>();
WsfDraw mDraw = WsfDraw();
bool mDiveDownFlag = true;
bool mBankLeftFlag = true;
end_script_variables
script bool Fly(WsfPlatform flyer, double heading, double altitude, double speed)
WsfGeoPoint pt = flyer.Location();
pt.Extrapolate(heading, 1852*3);
pt.Set(pt.Latitude(), pt.Longitude(), MATH.Max(altitude, mAltitudeMin));
if (mDrawEvasionVector == true)
{
mDraw.SetLayer("behavior_evade");
mDraw.SetDuration(PROCESSOR.UpdateInterval());
mDraw.SetColor(0,1,0);
mDraw.SetLineSize(1);
mDraw.SetLineStyle("dash_dot");
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(pt);
mDraw.End();
}
if (flyer.Mover().IsValid())
{
if (flyer.Mover().IsA_TypeOf("WSF_6DOF_MOVER"))
{
##flyer.GoToSpeed(speed);
##flyer.GoToAltitude(MATH.Max(altitude, mAltitudeMin), 200);
##flyer.TurnToHeading(heading, 500);
#flyer.GoToSpeed(speed);
#flyer.GoToLocation(pt);
flyer.GoToSpeed(speed);
double altRate = flyer.Speed() * MATH.Sin(40.0);
flyer.GoToAltitude(pt.Altitude(), altRate);
flyer.TurnToHeading(flyer.TrueBearingTo(pt), 500);
return true;
}
else if (flyer.Mover().IsA_TypeOf("WSF_AIR_MOVER"))
{
double delta = TIME_NOW - mLastTime;
mLastTime = TIME_NOW;
if (delta > cSLOW_UPDATE_INTERVAL)
{
delta = cSLOW_UPDATE_INTERVAL;
}
double deltaV = mVerticalAccel * delta;
if (altitude > PLATFORM.Altitude())
{
if (mClimbRate < 0.0)
{
//increase climb rate to a positive value (must keep pursuing a lower altitude though)
altitude = mAltitudeMin;
mClimbRate = mClimbRate + deltaV;
}
else if (mClimbRate < mMaxClimbRate)
{
//increase climb rate to max value (can pursue target altitude now)
mClimbRate = mClimbRate + deltaV;
}
}
else if (altitude < PLATFORM.Altitude())
{
if (mClimbRate > 0.0)
{
//decrease climb rate to a negative value (must keep pursuing a higher altitude though)
altitude = 9999999;
mClimbRate = mClimbRate - deltaV;
}
else if (mClimbRate > -mMaxClimbRate)
{
//decrease climb rate to max value (can pursue target altitude now)
mClimbRate = mClimbRate - deltaV;
}
}
double climbRateUsed = MATH.Fabs(mClimbRate);
flyer.GoToSpeed(speed);
flyer.GoToAltitude(altitude, climbRateUsed);
flyer.TurnToHeading(heading);
return true;
}
}
writeln_d(flyer.Name(), " aiai error: unknown or invalid mover for flight");
return false;
end_script
precondition
writeln_d(PLATFORM.Name(), " precondition evade, T=", TIME_NOW);
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
//always evade incoming weapons
int ownshipWeaponsIncoming = 0;
WsfProcessor proc = PLATFORM.Processor("incoming_threats");
if (proc.IsValid() && proc.ScriptExists("ThreatProcessorWeaponsIncoming"))
{
writeln_d("using incoming_threats threat processor!!!");
mIncoming = (Array<WsfPlatform>)(proc.Execute("ThreatProcessorWeaponsIncoming"));
ownshipWeaponsIncoming = mIncoming.Size();
}
else
{
writeln_d("using standard WSF_THREAT_PROCESSOR!!!");
mIncoming.Clear();
ownshipWeaponsIncoming = ((WsfRIPRProcessor)PROCESSOR).WeaponsIncoming(mIncoming);
}
if (ownshipWeaponsIncoming > 0)
{
//we now have weapons incoming, check to see if we are currently guiding any missiles with an uplink
//basically: check if we have any active weapons
int WeaponCount = ((WsfRIPRProcessor)PROCESSOR).WeaponsActive();
if (WeaponCount > 0)
{
writeln_d(PLATFORM.Name(), " weapons incoming, checking weapons active, count: ", WeaponCount);
double threat = 1000.0;
double asset = 1000.0;
//find most threatening incoming weapon
foreach(WsfPlatform p in mIncoming)
{
double range = PLATFORM.SlantRangeTo(p); # meters
double speed = p.Speed(); # meters/sec
double time = range / speed; # seconds
if (time < threat)
{
threat = time;
}
}
//find most valuable launched weapon (most threatening to enemy)
for (int i=0; i< WeaponCount; i=i+1)
{
WsfPlatform w = ((WsfRIPRProcessor)PROCESSOR).ActiveWeaponPlatform(i);
WsfTrack t = w.CurrentTargetTrack();
// include weapons we are uplinking to.
if (((WsfRIPRProcessor)PROCESSOR).IsUplinkingTo(w) &&
t.IsValid())
{
double range = w.SlantRangeTo(t); # meters
double speed = w.Speed(); # meters/sec
double time = range / speed; # seconds
if (time < asset)
{
asset = time;
}
}
}
extern double mEngagementAggressiveness;
//factor in our aggressiveness, and decide whether or not we should evade yet
double requiredAggressiveness = asset / (threat + asset);
writeln_d(PLATFORM.Name(), " threat: ", threat, ", asset: ", asset, ", required aggressiveness: ", requiredAggressiveness);
if (mEngagementAggressiveness < requiredAggressiveness)
{
#PROCESSOR.SetUpdateInterval(cFAST_UPDATE_INTERVAL);
return true;
}
else
{
Failure("my active weapons are closer, hold course");
}
}
else
{
#PROCESSOR.SetUpdateInterval(cFAST_UPDATE_INTERVAL);
return true;
}
}
else
{
writeln_d("no weapons incoming");
Failure("no weapons incoming");
}
#PROCESSOR.SetUpdateInterval(cSLOW_UPDATE_INTERVAL);
return false;
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing evade, T=", TIME_NOW);
//PLATFORM.Comment("evade");
### transition to evade incoming
PLATFORM.GoToSpeed(cEVADE_SPEED);
writeln_d(" GoToSpeed( ", cEVADE_SPEED," )");
extern double cDEFAULT_ALTITUDE;
double safeAltitudeToDiveTo = MATH.Max(mAltitudeMin, (cDEFAULT_ALTITUDE - mAltitudeToDiveEvade));
// Calculate a heading to evade all incoming missiles, weighted by distance, and then turn to it
double evadeHeading = 0;
double evadeAltitude = 0;
double evadeDivisor = 0;
double distMod = 0;
double bearingMod = 0;
int incomingCount = mIncoming.Size();
writeln_d(PLATFORM.Name(), " evade. dive/climb between altitudes: ", cDEFAULT_ALTITUDE, " <-> ", safeAltitudeToDiveTo);
writeln_d("evading ", incomingCount, " threats");
double y = 0;
double x = 0;
double dist = MATH.DOUBLE_MAX();
WsfPlatform nearestThreat;
// calculate an average heading to incoming platforms weighted by distance
for (int i = 0; i < incomingCount; i = i + 1)
{
WsfPlatform temp = (WsfPlatform)(mIncoming[i]);
if (!temp.IsValid() || (temp.Index() == PLATFORM.Index()))
{
continue;
}
double range = PLATFORM.SlantRangeTo(temp);
if (range < dist)
{
dist = range;
nearestThreat = (WsfPlatform)(mIncoming[i]);
}
if (range > 0)
{
distMod = 1 / range;
}
else
{
distMod = 1000000;
}
bearingMod = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(temp));
x = x + MATH.Sin(bearingMod) * distMod;
y = y + MATH.Cos(bearingMod) * distMod;
writeln_d(" Incoming ", temp.Name(), " at distance: ", 1 / distMod, ", bearing: ", bearingMod, ", x: ", MATH.Sin(bearingMod), ", y: ", MATH.Cos(bearingMod), ", (", temp.Latitude(), ", ", temp.Longitude(), ", ", temp.Altitude(), ") @ ", temp.Speed(), "m/s");
evadeDivisor = evadeDivisor + distMod;
evadeHeading = evadeHeading + bearingMod * distMod;
}
// get the GCI if there is one
WsfRIPRProcessor aiflProc = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
WsfRIPRProcessor gciProc;// = null;
WsfPlatform aiflPlat;// = null;
WsfPlatform gciPlat;// = null;
if (aiflProc.IsValid())
{
gciProc = aiflProc.GetRIPRCommanderProcessor();
aiflPlat = aiflProc.Platform();
}
if (gciProc.IsValid())
{
gciPlat = gciProc.Platform();
}
// if there's a gci, avoid centroids of all his subs
if (gciPlat.IsValid())
{
foreach (WsfPlatform aifl in gciPlat.Subordinates())
{
if (!aifl.IsValid() || aifl == PLATFORM || aifl == aiflPlat)
{
continue;
}
WsfGeoPoint centroid = aifl.GetSubsCentroid();
if (centroid.X() == 0 && centroid.Y() == 0 && centroid.Z() == 0)
{
continue;
}
distMod = 1 / (PLATFORM.SlantRangeTo(centroid));
bearingMod = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(centroid));
x = x + MATH.Sin(bearingMod) * distMod * weightPeersForEvade;
y = y + MATH.Cos(bearingMod) * distMod * weightPeersForEvade;
writeln_d(" AIFL ", aifl.Name(), " at distance: ", 1 / distMod, ", bearing: ", bearingMod, ", x: ", MATH.Sin(bearingMod), ", y: ", MATH.Cos(bearingMod), ", (", centroid.Latitude(), ", ", centroid.Longitude(), ", ", centroid.Altitude(), ")");
// but we'll avoid peers with less strength than we avoid incoming missiles
// so let's divide the distMod by 2
evadeDivisor = evadeDivisor + distMod;
evadeHeading = evadeHeading + bearingMod * distMod;
}
}
// otherwise, avoid members in your own squadron
else if (aiflPlat.IsValid())
{
foreach (WsfPlatform sub in aiflPlat.Subordinates())
{
if (!sub.IsValid() ||
sub == PLATFORM)
{
continue;
}
distMod = 1 / (PLATFORM.SlantRangeTo(sub));
bearingMod = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(sub));
x = x + MATH.Sin(bearingMod) * distMod * weightPeersForEvade;
y = y + MATH.Cos(bearingMod) * distMod * weightPeersForEvade;
writeln_d(" Peer ", sub.Name(), " at distance: ", 1 / distMod, ", bearing: ", bearingMod, ", x: ", MATH.Sin(bearingMod), ", y: ", MATH.Cos(bearingMod), ", (", sub.Latitude(), ", ", sub.Longitude(), ", ", sub.Altitude(), ") @ ", sub.Speed(), "m/s");
// but we'll avoid peers with less strength than we avoid incoming missiles
// so let's divide the distMod by 2
evadeDivisor = evadeDivisor + distMod;
evadeHeading = evadeHeading + bearingMod * distMod;
}
}
if (evadeDivisor == 0)
{
return;
}
// correct for quadrant
double theta = MATH.NormalizeAngle0_360(MATH.ATan(x/y));
if (y < 0)
{
theta = MATH.NormalizeAngle0_360(theta - 180);
}
writeln_d(" x: ", x, ", y: ", y, ", theta: ", theta);
evadeHeading = MATH.NormalizeAngle0_360(theta - 180);
if (mWobbleLeftRight == true)
{
//turn to angle to evade
if (mBankLeftFlag == true)
{
evadeHeading = MATH.NormalizeAngle0_360(theta - 180 - mBankAngleForEvading);
double headDiff = MATH.NormalizeAngleMinus180_180( evadeHeading - MATH.NormalizeAngle0_360(PLATFORM.Heading()) );
if (MATH.Fabs(headDiff) < 2.0 &&
nearestThreat.RelativeBearingTo(PLATFORM) > 0.0)
{
mBankLeftFlag = false;
}
}
else // bank right
{
evadeHeading = MATH.NormalizeAngle0_360(theta - 180 + mBankAngleForEvading);
double headDiff = MATH.NormalizeAngleMinus180_180(evadeHeading - MATH.NormalizeAngle0_360(PLATFORM.Heading()));
if (MATH.Fabs(headDiff) < 2.0 &&
nearestThreat.RelativeBearingTo(PLATFORM) < 0.0)
{
mBankLeftFlag = true;
}
}
}
evadeAltitude = MATH.Max(safeAltitudeToDiveTo, evadeAltitude);
if (mWobbleUpDown == true)
{
//compute the dive or climb
if (mDiveDownFlag == true)
{
writeln_d(PLATFORM.Name(), " diving to ", safeAltitudeToDiveTo);
evadeAltitude = safeAltitudeToDiveTo;
if (PLATFORM.Altitude() <= (cDEFAULT_ALTITUDE - 0.95*(cDEFAULT_ALTITUDE-safeAltitudeToDiveTo)))
{
mDiveDownFlag = false;
}
}
else
{
writeln_d(PLATFORM.Name(), " climbing to ", cDEFAULT_ALTITUDE);
evadeAltitude = cDEFAULT_ALTITUDE;
if (PLATFORM.Altitude() >= (safeAltitudeToDiveTo + 0.95*(cDEFAULT_ALTITUDE-safeAltitudeToDiveTo)))
{
mDiveDownFlag = true;
}
}
}
writeln_d(" Evading incoming at heading ", evadeHeading);
writeln_d(" Evading incoming, dive/climb to ", evadeAltitude);
Fly(PLATFORM, evadeHeading, evadeAltitude, cEVADE_SPEED);
if (mDrawNearestThreat)
{
mDraw.SetLayer("behavior_evade");
mDraw.SetDuration(PROCESSOR.UpdateInterval());
mDraw.SetColor(1,0,0);
mDraw.SetLineSize(5);
mDraw.SetLineStyle("dashed");
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(nearestThreat.Location());
mDraw.End();
}
end_execute
end_behavior

View File

@@ -0,0 +1,65 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior force_overshoot
script_variables
double cGRAVITY_ACCEL = 9.80665; # 1 G
double cDEFAULT_ACCEL = (cGRAVITY_ACCEL * 7.5); # m/s^2 ~7.5 Gs
double cMAX_RADIAL_ACCEL = 100 * cGRAVITY_ACCEL; # larger than possible, make the aircraft limit the turn
double cMAX_SPEED = 9999999999.99; # m/s
double mLastProcTime = -1.0;
end_script_variables
script bool RanLastTick()
if ((TIME_NOW-mLastProcTime) < 1.5*PROCESSOR.UpdateInterval())
{
return true;
}
else return false;
end_script
precondition
writeln("precondition force_overshoot");
//turn hard into threat
#extern WsfTrack mBiggestThreat;
if (mBiggestThreat.IsValid())
{
mLastProcTime = TIME_NOW;
return true;
}
mLastProcTime = -1.0;
return false;
end_precondition
execute
writeln("executing force_overshoot");
PLATFORM.Comment("force_overshoot");
//turn into the attacker as hard as possible
#extern WsfTrack mBiggestThreat;
//any out of plan manuevers? climb/dive ?
// no: match threat
#extern bool FlyTarget (WsfPlatform, WsfGeoPoint, double);
FlyTarget( PLATFORM, mBiggestThreat.CurrentLocation(), cMAX_SPEED);
end_execute
end_behavior

View File

@@ -0,0 +1,300 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior go_home
script_debug_writes off
script_variables
bool mDrawRoute = false; // draw the home route
WsfDraw mDraw = WsfDraw();
bool mCheckFuel = true; // check fuel levels
bool mCheckMyWeapons = true; // check the remaining quanitity of munitions on this platform
bool mCheckAllWeapons = false; // check the remaining quantity of munitions on all platforms in the peer group, including my own
bool mCheckUplinks = false; // check if uplinks are still existent
bool mWaitOnActiveWeapons = false; // don't go home as long as active weapons are being controlled
string mMainWeapon = ""; // main weapon of concern, not necessarily this platform's main weapon
bool mCheckEscorts = false; // check if escort platforms are still existent and on course
Array<string> mEscortNames = Array<string>(); // the names of platforms this platform is responsible for escorting
double cBINGO_SPEED = 516 * MATH.MPS_PER_NMPH(); // ~M0.8 or 248 m/s @ 25 kft#removed 1.5 * that was added to help 6dof movers 14Oct11
WsfGeoPoint mFirstKnownPoint = WsfGeoPoint();
string mRouteName = "";
int mRouteHomeIndex = 0; // the index of the waypoint on the default route the platform should return to
string mLastComment = "<none yet>";
end_script_variables
script bool HasEscorts()
WsfPlatform escortPlatform;
foreach ( string sEscortName in mEscortNames )
{
escortPlatform = WsfSimulation.FindPlatform(sEscortName);
if (escortPlatform.IsValid() )
{
return true;
}
}
return false;
end_script
script double NumWeaponsRemaining(WsfPlatform aPlatform)
double total = 0.0;
for (int i=0; i<aPlatform.WeaponCount(); i=i+1)
{
total = total + aPlatform.WeaponEntry(i).QuantityRemaining();
}
return total;
end_script
script void DrawRoute()
WsfRoute currRoute = PLATFORM.Route();
if (currRoute.IsValid())
{
//PLATFORM.Comment("draw current route : " + PLATFORM.Route().Name());
mDraw.SetLayer("behavior_go_home");
mDraw.Erase(PLATFORM.Name());
mDraw.SetId(PLATFORM.Name());
mDraw.SetColor(0,1,1);
mDraw.SetLineSize(2);
mDraw.SetLineStyle("dash_dot2");
mDraw.BeginPolyline();
for (int i=0; i<currRoute.Size(); i=i+1)
{
mDraw.Vertex(currRoute.Waypoint(i).Location());
}
mDraw.End();
}
end_script
on_init
mFirstKnownPoint = PLATFORM.Location();
end_on_init
precondition
writeln_d("precondition go_home");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
string msg = "";
bool result = false;
// go home if bingo on fuel
if (mCheckFuel == true &&
PLATFORM.FuelRemaining() < PLATFORM.FuelBingoQuantity())
{
msg = "GO HOME: bingo fuel";
result = true;
}
// go home if all my escorts are dead or gone
else if (mCheckEscorts == true &&
!HasEscorts())
{
msg = "GO HOME: all escorts are dead or gone";
result = true;
}
// DO NOT go home if I have any active weapons still under my control
else if (mWaitOnActiveWeapons == true &&
((WsfRIPRProcessor)PROCESSOR).WeaponsActive() <= 0)
{
msg = "GO HOME: no active weapons";
result = true;
}
// go home if there are no uplinks remaining
else if (mCheckUplinks == true &&
((WsfRIPRProcessor)PROCESSOR).UplinkCount() <= 0)
{
msg = "GO HOME: no active uplinks";
result = true;
}
// go home if all weapons, including my own, in the peer group are out of ammo
else if (mCheckAllWeapons == true &&
((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderPlatform().IsValid())
{
msg = "GO HOME: all weapons in group are out of ammo";
result = true;
foreach( WsfPlatform sub in ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderPlatform().Subordinates() )
{
if (mMainWeapon != "")
{
WsfWeapon temp = sub.Weapon(mMainWeapon);
if (temp.IsValid() &&
temp.QuantityRemaining() > 0)
{
msg = "";
result = false;
}
}
else if (NumWeaponsRemaining(sub) > 0.0)
{
msg = "";
result = false;
}
}
}
// go home if my weapons are out of ammo
else if (mCheckMyWeapons == true)
{
if (mMainWeapon != "")
{
WsfWeapon temp = PLATFORM.Weapon(mMainWeapon);
if (temp.IsValid() &&
temp.QuantityRemaining() <= 0)
{
msg = "GO HOME: main weapon out of ammo";
result = true;
}
}
else if (NumWeaponsRemaining(PLATFORM) <= 0.0)
{
msg = "GO HOME: out of weapons";
result = true;
}
}
// debug - why is this platform going home?
if (result)
{
writeln_d(" T=", TIME_NOW, " ", PLATFORM.Name(), " ", msg);
if (msg != mLastComment)
{
PLATFORM.Comment(msg);
mLastComment = msg;
}
return true;
}
return false;
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing go_home, T=", TIME_NOW);
//PLATFORM.Comment("go_home");
string msg = "";
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).CommanderProcessor();
if (commander.IsValid())
{
for (int i = 0; i < ((WsfRIPRProcessor)PROCESSOR).NumJobChannels(); i = i + 1)
{
commander.ClearBidsFor(((WsfRIPRProcessor)PROCESSOR), i);
}
}
// make sure we are at the right speed so we don't burn the fuel we have left too fast
PLATFORM.GoToSpeed(cBINGO_SPEED);
writeln_d(" GoToSpeed( ", cBINGO_SPEED," )");
bool onHomeRoute = false;
// if an egress route is defined, and that is the current route,
// find the closest waypoint and fly the route from there.
// this is mainly used to correct some bugs in the 6dof mover
if (mRouteName != "")
{
WsfRoute myRoute = PLATFORM.Route();
if(myRoute.Name() == "home_route")
{
int routeIndex = PLATFORM.RoutePointIndex();
if (routeIndex < 0 || routeIndex >= myRoute.Size())
{
routeIndex = 0;
}
double distThreshold = 4.0*185.2; ## 4/10th nm
if (myRoute.Waypoint(routeIndex).Location().GroundRangeTo(PLATFORM.Location()) < distThreshold)
{
routeIndex = routeIndex + 1;
if (routeIndex >= myRoute.Size())
{
routeIndex = 1;
}
}
onHomeRoute = PLATFORM.FollowRoute(myRoute, routeIndex);
#string msg = write_str("FollowRoute(home_route, ", routeIndex, ")");
#PLATFORM.Comment(msg);
}
// if there is an egress route defined, follow it
if (!onHomeRoute)
{
msg = write_str("attempting to construct and fly route: ", mRouteName);
//PLATFORM.Comment(msg);
writeln_d(" T=", TIME_NOW, " ", PLATFORM.Name(), " ", msg);
WsfRoute original = PLATFORM.Route();
WsfRoute homeRoute = WsfRoute.Create("home_route");
WsfRoute capRoute = WsfRoute.CopyGlobal(mRouteName);
double goHomeSpeed = cBINGO_SPEED;
#if (capRoute.Front().IsValid() && capRoute.Front().Speed() > 0)
#{
# goHomeSpeed = capRoute.Front().Speed();
#}
extern double cDEFAULT_ALTITUDE;
WsfGeoPoint startPoint = WsfGeoPoint.Construct(mFirstKnownPoint.Latitude(), mFirstKnownPoint.Longitude(), cDEFAULT_ALTITUDE);
homeRoute.Append(startPoint, goHomeSpeed);
capRoute.Transform(startPoint.Latitude(), startPoint.Longitude(), 0);
homeRoute.Append(capRoute);
onHomeRoute = PLATFORM.FollowRoute(homeRoute);
#PLATFORM.Comment("go_home FollowRoute(homeRoute)");
}
}
// if the platform is still not on an egress route, then there is not one
// defined or there was a problem with it, so one needs to be created.
if (!onHomeRoute)
{
if (PLATFORM.FollowRoute("DEFAULT_ROUTE", mRouteHomeIndex))
{
msg = write_str("should be flying default route");
//PLATFORM.Comment(msg);
writeln_d(" T=", TIME_NOW, " ", PLATFORM.Name(), " ", msg);
//PLATFORM.Comment("FollowRoute(DEFAULT_ROUTE, 1)");
}
else // what if no default route is provided? (fly to first known point)
{
#extern double cDEFAULT_ALTITUDE;
WsfGeoPoint tempPt = WsfGeoPoint.Construct(mFirstKnownPoint.Latitude(), mFirstKnownPoint.Longitude(), cDEFAULT_ALTITUDE);
PLATFORM.GoToLocation(tempPt);
msg = write_str("should be flying to point: ", mFirstKnownPoint.ToString());
//PLATFORM.Comment(msg);
writeln_d(" T=", TIME_NOW, " ", PLATFORM.Name(), " ", msg);
//PLATFORM.Comment("GoToLocation(tempPt)");
}
}
// debug
if (mDrawRoute)
{
DrawRoute();
}
end_execute
end_behavior

View File

@@ -0,0 +1,321 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior guide_weapons
script_debug_writes off
script_variables
//**********************************************************************//
//** parameters for guiding weapons **//
//**********************************************************************//
double mSensorAzimuthHalfAngle = 50.0; # degrees (represents uplink sensor)
//**********************************************************************//
//** parameters for approximating or flying target pursuit **//
//**********************************************************************//
double cINTERCEPT_SPEED = 1000.0; # m/s
double mOffsetAngle = 30.0; # degrees (for flying f-pole)
double mInterceptHeading = -1.0;
end_script_variables
precondition
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
mInterceptHeading = -1.0;
//writeln_d("precondition guide_weapons");
if (((WsfRIPRProcessor)PROCESSOR).UplinkCapable() && ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid())
{
//set current target, for main job (if its an uplink job)
//the pursue-target behavior does this for pursue jobs
WsfRIPRJob job = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(((WsfRIPRProcessor)PROCESSOR));
if (job.IsValid() && job.Name() == "weapon_uplink")
{
string targetName = (string)job.GetData("targetTrackName");
#extern WsfTrack GetTrackByName (WsfPlatform, string);
#extern bool TestTrackCategory (WsfTrack, string);
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (targetTrack.IsValid() && !TestTrackCategory(targetTrack, "unknown"))
{
((WsfRIPRProcessor)PROCESSOR).SetTarget(targetTrack);
}
}
bool haveUplinkJob = false;
for (int channel = 0; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
{
WsfRIPRJob aJob = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(((WsfRIPRProcessor)PROCESSOR), channel);
if (aJob.IsValid() && aJob.Name() == "weapon_uplink")
{
writeln_d("uplinking for job: ", aJob.GetDescription());
haveUplinkJob = true;
int targetIndex = (int)aJob.GetData("targetPlatformIndex");
WsfPlatform targetPlatform = WsfSimulation.FindPlatform(targetIndex);
mInterceptHeading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(targetPlatform));
break;
}
}
for (int up=0; up<((WsfRIPRProcessor)PROCESSOR).UplinkCount(); up = up+1)
{
WsfPlatform weaponPlatform = ((WsfRIPRProcessor)PROCESSOR).UplinkPlatformEntry(up);
if (weaponPlatform.IsValid())
{
writeln_d("uplinking to ", weaponPlatform.Name());
if (mInterceptHeading < 0)
{
WsfTrack weaponTarget = weaponPlatform.CurrentTargetTrack();
if (weaponTarget.IsValid() && weaponTarget.BelievedAlive())
{
mInterceptHeading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(weaponTarget));
}
}
}
}
if ((((WsfRIPRProcessor)PROCESSOR).UplinkCount() > 0) || (haveUplinkJob == true))
{
return true;
}
else
{
writeln_d(PLATFORM.Name(), " has no uplink jobs and no active uplinks!");
Failure("no uplink jobs found, & no active uplink");
}
}
else
{
writeln_d(PLATFORM.Name(), " is not uplink capable or has no commander!");
Failure("platform not uplink capable");
}
return false;
end_precondition
#on_init
#end_on_init
execute
writeln_d(PLATFORM.Name(), " executing guide_weapons, T=", TIME_NOW);
//PLATFORM.Comment("guide_weapons");
extern double cDEFAULT_ALTITUDE;
double interceptAltitude = cDEFAULT_ALTITUDE;
WsfTrack targetTrack = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
if (targetTrack.IsValid())
{
extern Array<Map<string, Object>> mWeaponArray;
#extern WsfTrack GetTrackByName (WsfPlatform, string);
#extern double GetWeaponRangeMax (WsfPlatform, Array<Map<string, Object>>);
#extern string CalculatePositioning (WsfPlatform, WsfTrack, double);
//perform a decent estimation of desired intercept angle
// (to use as a starting point for intelligent maneuvering for guiding missiles)
double ownSpeed = PLATFORM.Speed();
double targetSpeed = targetTrack.Speed();
double engageRangeMax = GetWeaponRangeMax(PLATFORM, mWeaponArray);
double slantRangeTo = PLATFORM.SlantRangeTo(targetTrack);
string positioning = CalculatePositioning(PLATFORM, targetTrack, 10.0);
//"pure" pursuit (by default)
mInterceptHeading = PLATFORM.TrueBearingTo(targetTrack);
if (((WsfRIPRProcessor)PROCESSOR).WeaponsActive(targetTrack) > 0)
{
//"f-pole" pursuit (fly offset)
#extern double MaximizeFPole(WsfPlatform, WsfTrack, double);
mInterceptHeading = MaximizeFPole(PLATFORM, targetTrack, mOffsetAngle);
}
else if (targetTrack.AirDomain() &&
slantRangeTo >= engageRangeMax &&
positioning != "head-to-head" &&
positioning != "head-to-tail" &&
targetSpeed >= ownSpeed)
{
//"lead" pursuit
WsfWaypoint interceptPoint = WsfWaypoint();
double timeToIntercept = PLATFORM.InterceptLocation3D(targetTrack, interceptPoint);
// If timeToIntercept is positive then we know intercept is possible
if (timeToIntercept > 0.0)
{
mInterceptHeading = interceptPoint.Heading();
}
}
if ((targetTrack.ElevationValid() || targetTrack.LocationValid()) && targetTrack.Altitude() > interceptAltitude)
{
interceptAltitude = targetTrack.Altitude();
}
if ( (interceptAltitude - PLATFORM.Altitude()) < 100)
{
interceptAltitude = PLATFORM.Altitude();
}
}
# else
# {
# writeln_d("no target track, not changing course for weapon guidance!");
# }
//double minYaw = PLATFORM.RelativeBearingTo(targetTrack);
//double maxYaw = minYaw;
double minYaw = MATH.NormalizeAngleMinus180_180(mInterceptHeading - PLATFORM.Heading());
double maxYaw = minYaw;
//first be sure to cover any uplinks against a current target
string targetName = ((WsfRIPRProcessor)PROCESSOR).GetTargetName();
double tMin = minYaw;
double tMax = maxYaw;
for (int up=0; up<((WsfRIPRProcessor)PROCESSOR).UplinkCount(); up = up+1)
{
WsfPlatform weaponPlatform = ((WsfRIPRProcessor)PROCESSOR).UplinkPlatformEntry(up);
if (weaponPlatform.IsValid())
{
WsfTrack weaponTarget = weaponPlatform.CurrentTargetTrack();
if (weaponTarget.TargetName() != targetName)
{
continue;
}
if (weaponTarget.IsValid() &&
weaponTarget.BelievedAlive() &&
((WsfRIPRProcessor)PROCESSOR).WeaponsActive(weaponTarget) )
{
tMin = MATH.Min(tMin, PLATFORM.RelativeBearingTo(weaponTarget));
tMax = MATH.Max(tMax, PLATFORM.RelativeBearingTo(weaponTarget));
double coverage = tMax - tMin;
if ((mSensorAzimuthHalfAngle*2*0.9) > coverage) //take off 10% for padding
{
//can cover this target
minYaw = tMin;
maxYaw = tMax;
}
else
{
//can NOT cover this target, drop the uplink.
string msg = write_str(PLATFORM.Name(), " CAN NOT SUPPORT ACTIVE UPLINK AGAINST ", weaponTarget.TargetName());
//PLATFORM.Comment(msg);
writeln_d(msg);
}
}
}
}
//iterate over current uplinks, find relative angles, adjust bounds
for (int up=0; up<((WsfRIPRProcessor)PROCESSOR).UplinkCount(); up = up+1)
{
WsfPlatform weaponPlatform = ((WsfRIPRProcessor)PROCESSOR).UplinkPlatformEntry(up);
if (weaponPlatform.IsValid())
{
WsfTrack weaponTarget = weaponPlatform.CurrentTargetTrack();
if (weaponTarget.IsValid() &&
weaponTarget.BelievedAlive() &&
((WsfRIPRProcessor)PROCESSOR).WeaponsActive(weaponTarget) )
{
tMin = MATH.Min(tMin, PLATFORM.RelativeBearingTo(weaponTarget));
tMax = MATH.Max(tMax, PLATFORM.RelativeBearingTo(weaponTarget));
double coverage = tMax - tMin;
if ((mSensorAzimuthHalfAngle*2*0.9) > coverage) //take off 10% for padding
{
//can cover this target
minYaw = tMin;
maxYaw = tMax;
}
else
{
//can NOT cover this target, drop the uplink
string msg = write_str(PLATFORM.Name(), " CAN NOT SUPPORT ACTIVE UPLINK AGAINST ", weaponTarget.TargetName());
//PLATFORM.Comment(msg);
writeln_d(msg);
}
}
}
}
if (((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid())
{
for (int channel = 1; channel < ((WsfRIPRProcessor)PROCESSOR).GetNumJobChannels(); channel = channel + 1)
{
double tempMin = minYaw;
double tempMax = maxYaw;
WsfRIPRJob aJob = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(((WsfRIPRProcessor)PROCESSOR), channel);
if (!aJob.IsValid())
{
continue;
}
if (aJob.Name() == "weapon_uplink")
{
string targetName = (string)aJob.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (!targetTrack.IsValid() || !targetTrack.BelievedAlive())
{
continue;
}
if (((WsfRIPRProcessor)PROCESSOR).WeaponsActive(targetTrack) <= 0)
{
continue;
}
tempMin = MATH.Min(tempMin, PLATFORM.RelativeBearingTo(targetTrack)); //[-180,180]
tempMax = MATH.Max(tempMax, PLATFORM.RelativeBearingTo(targetTrack)); //[-180,180]
double neededCoverage = tempMax - tempMin;
double sensorCoverage = mSensorAzimuthHalfAngle * 2 * 0.9; //take 10% off for padding
if (sensorCoverage > neededCoverage)
{
//can cover this target
minYaw = tempMin;
maxYaw = tempMax;
}
else
{
//can NOT cover this target, drop the uplink.
string msg = write_str(PLATFORM.Name(), " CAN NOT SUPPORT ACTIVE UPLINK AGAINST ", targetTrack.TargetName());
//PLATFORM.Comment(msg);
writeln_d(msg);
}
}
}
}
// now minYaw and maxYaw define relative headings that must be included by our sensor coverage
// use the desired interceptHeading as a starting point
// adjust if necessary according to our mSensorAzimuthHalfAngle
double relativeInterceptHeading = MATH.NormalizeAngleMinus180_180(mInterceptHeading - PLATFORM.Heading());
writeln_d("minyaw: ", minYaw, ", maxyaw: ", maxYaw);
if (minYaw < (relativeInterceptHeading - mSensorAzimuthHalfAngle))
{
//adjust a bit to the left
relativeInterceptHeading = minYaw + mSensorAzimuthHalfAngle*0.9;
writeln_d("intercept heading BEFORE adjusting for active weapons: ", mInterceptHeading);
mInterceptHeading = MATH.NormalizeAngleMinus180_180(PLATFORM.Heading() + relativeInterceptHeading);
writeln_d("intercept heading AFTER adjusting for active weapons: ", mInterceptHeading);
}
else if (maxYaw > (relativeInterceptHeading + mSensorAzimuthHalfAngle))
{
//adjust a bit to the right
relativeInterceptHeading = maxYaw - mSensorAzimuthHalfAngle*0.9;
writeln_d("intercept heading BEFORE adjusting for active weapons: ", mInterceptHeading);
mInterceptHeading = MATH.NormalizeAngleMinus180_180(PLATFORM.Heading() + relativeInterceptHeading);
writeln_d("intercept heading AFTER adjusting for active weapons: ", mInterceptHeading);
}
#extern bool FlyHold (WsfPlatform, double, double, double);
FlyHold( PLATFORM, mInterceptHeading, interceptAltitude, cINTERCEPT_SPEED);
end_execute
end_behavior

View File

@@ -0,0 +1,127 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
#this is a parent behavior of any defensive manuevers
# first determine if we are in danger, then defend in the child behaviors
include_once behavior_unload.txt
include_once behavior_force_overshoot.txt
behavior in_danger
script_variables
double cANGLE_TOLERANCE = 45.0; # degrees
WsfTrack mBiggestThreat;
end_script_variables
script bool NotWeapon(WsfTrack aTrack)
if (aTrack.Target().IsValid() && aTrack.Target().WeaponEngagement().IsValid())
{
return false; //it is a weapon
}
return true;
end_script
script bool IsFlyer(WsfPlatform aPlat)
if (!aPlat.IsValid())
{
return false;
}
if (aPlat.Mover().IsA_TypeOf("WSF_AIR_MOVER" ) ||
aPlat.Mover().IsA_TypeOf("WSF_6DOF_MOVER") )
{
return true;
}
else if (aPlat.MakeTrack().AirDomain())
{
return true;
}
else if((aPlat.Altitude() > 152.4) && (aPlat.Speed() > 51.44) ) // ~500 feet alt, ~100 knots speed
{
return true;
}
return false;
end_script
precondition
writeln("precondition in_danger");
//check track list for any threats
#extern double mVisualRange;
#extern string CalculatePositioning( WsfPlatform, WsfTrack, double );
//see if anybody is threatening me
mBiggestThreat = null;
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
if (!t.SideValid() || t.Side() != PLATFORM.Side())
{
double range = PLATFORM.SlantRangeTo(t);
if ((range < mVisualRange) && NotWeapon(t) && IsFlyer(t.Target()))
{
string pos = CalculatePositioning(PLATFORM, t, cANGLE_TOLERANCE );
if (pos=="tail-to-head" || pos=="target-facing-me")
{
if (!mBiggestThreat.IsValid() || (range < PLATFORM.SlantRangeTo(mBiggestThreat)))
{
#mBiggestThreat = (WsfTrack)t;
mBiggestThreat = t;
}
}
}
}
}
if (mBiggestThreat.IsValid())
{
//if we are closer to our target, stay in the fight
WsfTrack targetTrack = PROCESSOR.GetTarget();
if (!targetTrack.IsNull() && targetTrack.IsValid())
{
#extern double mEngagementAggressiveness;
double threatRange = PLATFORM.SlantRangeTo(mBiggestThreat);
double requiredAggressiveness = threatRange / (threatRange + PLATFORM.SlantRangeTo(targetTrack));
if (mEngagementAggressiveness >= requiredAggressiveness)
{
string myPos = CalculatePositioning(PLATFORM, targetTrack, cANGLE_TOLERANCE );
if (myPos=="head-to-tail" || myPos="me-facing-target")
{
writeln(" not in enough danger, I am chasing target: ", targetTrack.TargetName());
return false; #do not defend, I am aggressive enough to stay after my target, and I am facing my target
}
}
}
return true;
}
return false;
end_precondition
execute
writeln("executing in_danger");
PLATFORM.Comment("in_danger");
end_execute
selector
behavior unload end_behavior
behavior force_overshoot end_behavior
end_selector
end_behavior

View File

@@ -0,0 +1,59 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior low_speed_recovery
script_variables
double mLastAltitude = 0.0;
double mMinimumSpeedAllowed = 102.9; # units: meters/sec, == 200 knots
double cDIVE_ALTITUDE = 304.8; # 1000 feet
double cMAX_SPEED = 9999999999.99; # m/s
double cGRAVITY_ACCEL = 9.80665; # 1 G
double cDEFAULT_ACCEL = (cGRAVITY_ACCEL * 7.5); # m/s^2 ~7.5 Gs
end_script_variables
precondition
writeln("precondition low_speed_recovery");
WsfTrack targetTrack = PROCESSOR.GetTarget();
WsfMover mover = PLATFORM.Mover();
if (targetTrack.IsNull() || !targetTrack.IsValid() || mover.IsNull() || !mover.IsValid())
{
return false;
}
if (PLATFORM.Speed() < mMinimumSpeedAllowed)
{
return true;
}
return false;
end_precondition
execute
writeln("executing low_speed_recovery");
PLATFORM.Comment("low_speed_recovery");
//straighten out, fly forward, and negative 1 G to minimize lift drag
PLATFORM.GoToSpeed(cMAX_SPEED, cDEFAULT_ACCEL, true); # go max speed
writeln(" GoToSpeed( ", cMAX_SPEED," )");
if (PLATFORM.Pitch()>0)
{
PLATFORM.GoToAltitude(cDIVE_ALTITUDE, cGRAVITY_ACCEL, false); # unload, dive "slowly" at 1 negative G so there is zero lift drag
}
end_execute
end_behavior

View File

@@ -0,0 +1,31 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior manage-fuel
script_debug_writes off
script_variables
end_script_variables
precondition
writeln_d("precondition manage-fuel");
return false;
end_precondition
execute
writeln_d("executing manage-fuel");
end_execute
end_behavior

View File

@@ -0,0 +1,31 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior manage-signature
script_debug_writes off
script_variables
end_script_variables
precondition
writeln_d("precondition manage-signature");
return false;
end_precondition
execute
writeln_d("executing manage-signature");
end_execute
end_behavior

View File

@@ -0,0 +1,325 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pincer
script_debug_writes off
script_variables
//debug parameters
bool mDrawSteering = false;
//parameters for how to initially fly pincer (1st phase, separation)
string mDirection = "none";
double mPincerSpeed = 500 * MATH.MPS_PER_NMPH(); //400 kts
double mPincerOffsetAngle = 70.0; //degrees
//parameters for determining if to chase and how to do so (2nd phase, option A)
double mEnemyFOVHalfAngle = 50.0; //degrees
double mChaseAngle = 75.0; //degrees
double mChaseSpeedlnsideFOV = 600 * MATH.MPS_PER_NMPH(); //600 kts
double mChaseSpeedOutsideFOV = 1200 * MATH.MPS_PER_NMPH(); //1200 kts
//parameters for determining if to drag (run) and how to do so (2nd phase, option B)
double mRunDistance = 130 * 1852; //130nm (meters)
double mRunAngleLimit = 15.0; //degrees
double mDragSpeed = 800 * MATH.MPS_PER_NMPH(); //800 kts
double mDragAddedAngle = -1.0; //degrees
//util var, not for user edit
WsfGeoPoint mPincerPoint;
bool mDragging = false;
bool mChasing = false;
WsfDraw mDraw = WsfDraw();
WsfRIPRJob mCurrentJob;
end_script_variables
script double GetPincerDragOffset(WsfTrack aTgt)
if (aTgt.Speed() >= PLATFORM.Speed())
{
return 180.0;
}
else
{
return (180 - MATH.ACos(aTgt.Speed()/PLATFORM.Speed()));
}
end_script
precondition
#writeln_d("precondition pincer");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
if (((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid())
{
mCurrentJob = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
if (mCurrentJob.IsValid())
{
// If we're supposed to fly towards a point, go for it!
if (mCurrentJob.Name() == "pincer")
{
mPincerPoint = (WsfGeoPoint)mCurrentJob.GetData("ZonePoint");
mDirection = (string)mCurrentJob.GetData(PLATFORM.Name());
if (mPincerPoint.IsValid())
{
return true;
}
else
{
writeln_d(PLATFORM.Name(), " pincer point not valid!");
return Failure("pincer job did not have valid point");
}
}
else
{
writeln_d(PLATFORM.Name(), " job not a pincer job: ", mCurrentJob.Name());
}
}
else
{
writeln_d(PLATFORM.Name(), " job not valid!");
}
}
else
{
writeln_d(PLATFORM.Name(), " commander not found!");
return Failure("no commander found");
}
return Failure("current job not a valid pincer job");
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing pincer!, T=", TIME_NOW);
//provide for another channel for pursue-target jobs
if (((WsfRIPRProcessor)PROCESSOR).NumJobChannels() <= 1)
{
((WsfRIPRProcessor)PROCESSOR).SetNumJobChannels(2);
}
//writelnd("executing pincer.");
((WsfRIPRProcessor)PROCESSOR).ClearTarget();
//calculate heading if we are still flying offset
double angle = 0;
if (mDirection == "left")
{
angle = -mPincerOffsetAngle;
}
else if (mDirection == "right")
{
angle = mPincerOffsetAngle;
}
double heading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(mPincerPoint) + angle);
//find out if we are being chased & should drag threats away
WsfTrack worstTrack;
double worstDist = MATH.DOUBLE_MAX();
double worstFOV = MATH.DOUBLE_MAX();
Array<string> targetNames = (Array<string>)mCurrentJob.GetData("ZoneThreatNameArray");
#extern WsfTrack GetTrackByName (WsfPlatform, string);
int allOnCap = (int)mCurrentJob.GetData("all_on_cap");
if (allOnCap <= 0)
{
//save off the closest opponent track
foreach(string tgtName in targetNames)
{
WsfTrack tgt = GetTrackByName(PLATFORM, tgtName);
if (tgt.IsValid() && tgt.LocationValid())
{
double attackRange = tgt.GroundRangeTo(PLATFORM);
if (attackRange < worstDist)
{
//run away, drag threats out
worstDist = attackRange;
worstTrack = tgt;
}
double fov = MATH.Fabs(tgt.RelativeBearingTo(PLATFORM));
if (fov < worstFOV)
{
worstFOV = fov;
}
}
}
//manage dragging state
if ( (mDragging == false) &&
(worstDist < mRunDistance) &&
(worstTrack.IsValid()) &&
(worstTrack.HeadingValid()) &&
(MATH.Fabs(worstTrack.TrueBearingTo(PLATFORM)-worstTrack.Heading()) < mRunAngleLimit))
{
mDragging = true;
if (mDirection == "right")
{
int count = (int)mCurrentJob.GetData("right_drag");
mCurrentJob.SetData("right_drag",count+1);
writeln_d(PLATFORM.Name(), " increased right_drag to: ", count+1);
}
else if (mDirection == "left")
{
int count = (int)mCurrentJob.GetData("left_drag");
mCurrentJob.SetData("left_drag",count+1);
writeln_d(PLATFORM.Name(), " increased left_drag to: ", count+1);
}
}
else if ( (mDragging == true) &&
( (!worstTrack.IsValid()) ||
(worstDist > (mRunDistance*1.1)) ||
(!worstTrack.HeadingValid()) ||
(MATH.Fabs(worstTrack.TrueBearingTo(PLATFORM)-worstTrack.Heading()) > (mRunAngleLimit*1.1))))
{
mDragging = false;
if (mDirection == "right")
{
int count = (int)mCurrentJob.GetData("right_drag");
mCurrentJob.SetData("right_drag",count-1);
writeln_d(PLATFORM.Name(), " decreased right_drag to: ", count-1);
}
else if (mDirection == "left")
{
int count = (int)mCurrentJob.GetData("left_drag");
mCurrentJob.SetData("left_drag",count -1);
writeln_d(PLATFORM.Name(), " decreased left_drag to: ", count-1);
}
}
mChasing = false;
if (mDragging == true)
{
writeln_d("dragging!");
//try to maintain distance
PLATFORM.GoToSpeed(mDragSpeed);
double dragOffset = GetPincerDragOffset(worstTrack);
if (mDirection == "right")
{
if (mDragAddedAngle > 0)
{
heading = MATH.NormalizeAngle0_360(heading + mDragAddedAngle);
}
else
{
heading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(mPincerPoint) + dragOffset);
}
}
else if (mDirection == "left")
{
if (mDragAddedAngle > 0)
{
heading = MATH.NormalizeAngle0_360(heading - mDragAddedAngle);
}
else
{
heading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(mPincerPoint) - dragOffset);
}
}
}
else
{
//look for reasons to chase
PLATFORM.GoToSpeed(mPincerSpeed);
//check if other pair is dragging, so we should chase
int count = 0;
if (mDirection == "right")
{
count = (int)mCurrentJob.GetData("left_drag");
}
else if (mDirection == "left")
{
count = (int)mCurrentJob.GetData("right_drag");
}
double diff = 0;
if (worstTrack.IsValid() && worstTrack.HeadingValid())
{
double b1 = worstTrack.Heading();
double b2 = worstTrack.TrueBearingTo(PLATFORM.Location());
diff = MATH.NormalizeAngleMinus180_180(b2 - b1);
}
if (count > 0 && MATH.Fabs(diff) > mEnemyFOVHalfAngle)
{
//the other pincer group is being engaged, turn in now
writeln_d(PLATFORM.Name(), " chasing because other pair is dragging");
mChasing = true;
}
//check if we have flanked & can now chase
else
{
if (mDirection == "right" && diff < -mChaseAngle)
{
writeln_d(PLATFORM.Name(), " right chasing because past chase angle");
mChasing = true;
}
else if (mDirection == "left" && diff > mChaseAngle)
{
mChasing = true;
writeln_d(PLATFORM.Name(), " left chasing because past chase angle");
}
}
}
}
else
{
writeln_d("All targets still on cap, no dragging or chasing yet!");
}
if (mChasing == true)
{
writeln_d("chasing!");
//double FOV = MATH.Fabs(worstTrack.RelativeBearingTo(PLATFORM));
if (worstFOV <= mEnemyFOVHalfAngle)
{
PLATFORM.GoToSpeed(mChaseSpeedlnsideFOV);
}
else
{
PLATFORM.GoToSpeed(mChaseSpeedOutsideFOV);
}
if (worstTrack.IsValid())
{
heading = PLATFORM.TrueBearingTo(worstTrack);
((WsfRIPRProcessor)PROCESSOR).SetTarget(worstTrack);
}
else
{
heading = PLATFORM.TrueBearingTo((WsfGeoPoint)mCurrentJob.GetData("ZonePoint"));
}
}
PLATFORM.TurnToHeading(heading);
if (mDrawSteering == true)
{
WsfGeoPoint pt = PLATFORM.Location();
pt.Extrapolate(heading, 185200);
mDraw.SetLayer("behavior_pincer");
mDraw.SetDuration(PROCESSOR.UpdateInterval());
mDraw.SetColor(0.0, 1.0, 0.75); //green-blue
mDraw.SetLineSize(1);
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(pt);
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(mPincerPoint);
mDraw.End();
}
# # writeln_d(" T=", TIME_NOW, ", range: ", interceptRange, " true-bearing; interceptHeading); #
##if ( (interceptAltitude - PLATFORM.Altitude()) > 5) #if ( (interceptAltitude - PLATFORM.Altitude()) > 10000*MATH.M_PER_FT())
#{
# writeln_d("GoToAltitude: ",interceptAltitude); # #PLATF0RM.GoToAltitude(interceptAltitude);
#>
end_execute
end_behavior

View File

@@ -0,0 +1,363 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pincer_fsm
script_debug_writes off
script_variables
//debug parameters
bool mDrawSteering = false;
//parameters for how to initially fly pincer (1st phase, separation)
string mDirection = "none";
double mPincerSpeed = 500 * MATH.MPS_PER_NMPH(); //400 kts
double mPincerOffsetAngle = 70.0; //degrees
//parameters for determining if to chase and how to do so (2nd phase, option A)
double mEnemyFOVHalfAngle = 50.0; //degrees
double mChaseAngle = 75.0; //degrees
double mChaseSpeedlnsideFOV = 600 * MATH.MPS_PER_NMPH(); //600 kts
double mChaseSpeedOutsideFOV = 1200 * MATH.MPS_PER_NMPH(); //1200 kts
//parameters for determining if to drag (run) and how to do so (2nd phase, option B)
double mRunDistance = 130 * 1852; //130nm (meters)
double mRunAngleLimit = 15.0; //degrees
double mDragSpeed = 800 * MATH.MPS_PER_NMPH(); //800 kts
double mDragAddedAngle = -1.0; //degrees
// bidding parameters (similar to pursue-target parameters)
double cMIN_JOB_BID = -MATH.DOUBLE_MAX();
# double cWEIGHT_CURRENT_TARGET = 10.0;
# double cWEIGHT_CLOSING_SPEED_OF = 0.5; #1.0;
# double cWEIGHT_SLANT_RANGE_TO = -4.0; #-3.0;
# double cWEIGHT_FUEL = 2.0;
# double cWEIGHT_WEAPONS_IN_FLIGHT = 0.0;
# double cWEIGHT_ANY_WEAPONS_ACTIVE = 20.0;
# double cWEIGHT_THREAT_WEAPONS_ENVELOPE = 10000.0;
# double cBASE_SLANT_RANGE_CONSTANT = 600000.0; //over 300 nm away [How is this used???]
# double cBASE_CLOSING_SPEED_CONSTANT = 1050.0; //over 2000 kts closing speed
double cMAX_SLANT_RANGE = 1000000.0; #over 539 nm away, target ranges beyond this are considered unfavorable
double cWEIGHT_SLANT_RANGE_TO = 1.0;
double cMIN_CLOSING_SPEED = -1050.0; #threats running away (negative closing) faster are considered unfavorable
double cWEIGHT_CLOSING_SPEED = 1.0; #scale for how closing speed translates to distance
double cWEIGHT_FUEL = 0.0; #try a value of 2.0 if you care about fuel
double cWEIGHT_MY_WEAPONS_ACTIVE = 0.0; #changes bid if your own weapons are active on target
double cWEIGHT_PEERS_WEAPONS_ACTIVE = 0.0; #changes bid if your peers weapons are active on target
double cWEIGHT_THREAT_WEAPONS_ENVELOPE = 10000.0; #uses the global "mWeaponsEnvelope" array, try value of 10000 if you care about that
//careful with these two parameters, they are stateful
double cWEIGHT_CURRENT_TARGET = 0.0; #changes bid if you are currently targeting the threat
//util var, not for user edit
WsfGeoPoint mPincerPoint;
WsfDraw mDraw = WsfDraw();
WsfRIPRJob mCurrentJob;
WsfTrack mWorstTrack;
double mWorstDist = MATH.DOUBLE_MAX();
double mWorstFOV = MATH.DOUBLE_MAX();
end_script_variables
script double GetPincerDragOffset(WsfTrack aTgt)
if (aTgt.Speed() >= PLATFORM.Speed())
{
return 180.0;
}
else
{
return (180 - MATH.ACos(aTgt.Speed()/PLATFORM.Speed()));
}
end_script
precondition
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
#writeln_d("precondition pincer");
if (((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid())
{
mCurrentJob = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
if (mCurrentJob.IsValid())
{
// If we're supposed to fly towards a point, go for it!
if (mCurrentJob.Name() == "pincer")
{
mPincerPoint = (WsfGeoPoint)mCurrentJob.GetData("ZonePoint");
mDirection = (string)mCurrentJob.GetData(PLATFORM.Name());
if (mPincerPoint.IsValid())
{
return true;
}
else
{
writeln_d(PLATFORM.Name(), " pincer point not valid!");
return Failure("pincer job did not have valid point");
}
}
else
{
writeln_d(PLATFORM.Name(), " job not a pincer job: ", mCurrentJob.Name());
}
}
else
{
writeln_d(PLATFORM.Name(), " job not valid!");
}
}
else
{
writeln_d(PLATFORM.Name(), " commander not found!");
return Failure("no commander found");
}
return Failure("current job not a valid pincer job");
end_precondition
### FSM states are evaluated after "execute"
execute
writeln_d(PLATFORM.Name(), " executing pincer!, T=", TIME_NOW);
//provide for another channel for pursue-target jobs
if (((WsfRIPRProcessor)PROCESSOR).NumJobChannels() <= 1)
{
((WsfRIPRProcessor)PROCESSOR).SetNumJobChannels(2);
}
((WsfRIPRProcessor)PROCESSOR).ClearTarget();
//save off the closest opponent track
mWorstDist = MATH.DOUBLE_MAX();
mWorstFOV = MATH.DOUBLE_MAX();
Array<string> targetNames = (Array<string>)mCurrentJob.GetData("ZoneThreatNameArray");
foreach(string tgtName in targetNames)
{
WsfTrack tgt = GetTrackByName(PLATFORM, tgtName);
if (tgt.IsValid() && tgt.LocationValid())
{
double attackRange = tgt.GroundRangeTo(PLATFORM);
if (attackRange < mWorstDist)
{
//run away, drag threats out
mWorstDist = attackRange;
mWorstTrack = tgt;
}
double fov = MATH.Fabs(tgt.RelativeBearingTo(PLATFORM));
if (fov < mWorstFOV)
{
mWorstFOV = fov;
}
}
}
# if (mDrawSteering == true)
# {
# WsfGeoPoint pt = PLATFORM.Location();
# pt.Extrapolate(heading, 185200);
# mDraw.SetLayer("behavior_pincer");
# mDraw.SetDuration(((WsfRIPRProcessor)PROCESSOR).UpdateInterval());
# mDraw.SetColor(0.0, 1.0, 0.75); //green-blue
# mDraw.SetLineSize(1);
# mDraw.BeginLines();
# mDraw.Vertex(PLATFORM.Location());
# mDraw.Vertex(pt);
# mDraw.Vertex(PLATFORM.Location());
# mDraw.Vertex(mPincerPoint);
# mDraw.End();
# }
end_execute
script bool ShouldChase()
int allOnCap = (int)mCurrentJob.GetData("all_on_cap");
if (allOnCap > 0)
{
return false;
}
int other_dragging_count = 0;
if (mDirection == "right")
{
other_dragging_count = (int)mCurrentJob.GetData("left_drag");
}
else if (mDirection == "left")
{
other_dragging_count = (int)mCurrentJob.GetData("right_drag");
}
double diff = 0;
if (mWorstTrack.IsValid() && mWorstTrack.HeadingValid())
{
double b1 = mWorstTrack.Heading();
double b2 = mWorstTrack.TrueBearingTo(PLATFORM.Location());
diff = MATH.NormalizeAngleMinus180_180(b2 - b1);
}
if (other_dragging_count > 0 && MATH.Fabs(diff) > mEnemyFOVHalfAngle)
{
//the other pincer group is being engaged, turn in now
writeln_d(PLATFORM.Name(), " chasing because other pair is dragging");
return true;
}
else if ((mDirection == "right" && diff < -mChaseAngle) ||
(mDirection == "left" && diff > mChaseAngle) )
{
//we have already flanked, so now we can chase (regardless of other side dragging or not)
writeln_d(PLATFORM.Name(), " chasing because past chase angle");
return true;
}
return false;
end_script
#show_state_transitions
#first default state, start here and go back to here if confused
state SEPARATE
next_state DRAG
int allOnCap = (int)mCurrentJob.GetData("all_on_cap");
if ( (allOnCap <= 0) &&
(mWorstDist < mRunDistance) &&
(mWorstTrack.IsValid()) &&
(mWorstTrack.HeadingValid()) &&
(MATH.Fabs(mWorstTrack.TrueBearingTo(PLATFORM)-mWorstTrack.Heading()) < mRunAngleLimit))
{
return true;
}
return false;
end_next_state
next_state CHASE
return ShouldChase();
end_next_state
next_state SEPARATE
double separate_angle = 0;
if (mDirection == "left")
{
separate_angle = -mPincerOffsetAngle;
}
else if (mDirection == "right")
{
separate_angle = mPincerOffsetAngle;
}
double heading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(mPincerPoint) + separate_angle);
PLATFORM.GoToSpeed(mPincerSpeed);
PLATFORM.TurnToHeading(heading);
return true;
end_next_state
end_state
state DRAG
next_state SEPARATE
if ( (!mWorstTrack.IsValid()) ||
(mWorstDist > (mRunDistance*1.1)) ||
(!mWorstTrack.HeadingValid()) ||
(MATH.Fabs(mWorstTrack.TrueBearingTo(PLATFORM)-mWorstTrack.Heading()) > (mRunAngleLimit*1.1)))
{
return true;
}
return false;
end_next_state
next_state DRAG
writeln_d("dragging!");
//try to maintain distance
PLATFORM.GoToSpeed(mDragSpeed);
double dragOffset = GetPincerDragOffset(mWorstTrack);
double heading;
if (mDirection == "right")
{
if (mDragAddedAngle > 0)
{
heading = MATH.NormalizeAngle0_360(heading + mDragAddedAngle);
}
else
{
heading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(mPincerPoint) + dragOffset);
}
}
else if (mDirection == "left")
{
if (mDragAddedAngle > 0)
{
heading = MATH.NormalizeAngle0_360(heading - mDragAddedAngle);
}
else
{
heading = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(mPincerPoint) - dragOffset);
}
}
PLATFORM.TurnToHeading(heading);
return true;
end_next_state
on_entry
if (mDirection == "right")
{
int count = (int)mCurrentJob.GetData("right_drag");
mCurrentJob.SetData("right_drag",count+1);
writeln_d(PLATFORM.Name(), " increased right_drag to: ", count+1);
}
else if (mDirection == "left")
{
int count = (int)mCurrentJob.GetData("left_drag");
mCurrentJob.SetData("left_drag",count+1);
writeln_d(PLATFORM.Name(), " increased left_drag to: ", count+1);
}
end_on_entry
on_exit
if (mDirection == "right")
{
int count = (int)mCurrentJob.GetData("right_drag");
mCurrentJob.SetData("right_drag",count-1);
writeln_d(PLATFORM.Name(), " decreased right_drag to: ", count-1);
}
else if (mDirection == "left")
{
int count = (int)mCurrentJob.GetData("left_drag");
mCurrentJob.SetData("left_drag",count -1);
writeln_d(PLATFORM.Name(), " decreased left_drag to: ", count-1);
}
end_on_exit
end_state
state CHASE
next_state SEPARATE
return ! ShouldChase();
end_next_state
next_state CHASE
writeln_d("chasing!");
if (mWorstFOV <= mEnemyFOVHalfAngle)
{
PLATFORM.GoToSpeed(mChaseSpeedlnsideFOV);
}
else
{
PLATFORM.GoToSpeed(mChaseSpeedOutsideFOV);
}
double heading = 0;
if (mWorstTrack.IsValid())
{
heading = PLATFORM.TrueBearingTo(mWorstTrack);
((WsfRIPRProcessor)PROCESSOR).SetTarget(mWorstTrack);
}
else
{
heading = PLATFORM.TrueBearingTo((WsfGeoPoint)mCurrentJob.GetData("ZonePoint"));
}
PLATFORM.TurnToHeading(heading);
return true;
end_next_state
end_state
end_behavior

View File

@@ -0,0 +1,108 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pursue-point
script_debug_writes off
script_variables
bool tempVariable;
bool mConsiderFuel = false;
WsfGeoPoint mCurrentPointIntercept;
#double cSLOW_UPDATE_RATE = 3.0;
double cINTERCEPT_SPEED = 1000; # m/s
string mOldPointStr = "no point";
end_script_variables
precondition
writeln_d("precondition pursue-point");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
string anOldPointStr = mOldPointStr;
mOldPointStr = "no point";
if (((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().IsValid())
{
WsfRIPRJob currentJob = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
if (currentJob.IsValid())
{
// If we're supposed to fly towards a point, go for it!
if (currentJob.Name() == "pursue-point")
{
mCurrentPointIntercept = (WsfGeoPoint)currentJob.GetData("targetPoint");
if (mCurrentPointIntercept.IsValid())
{
########################################################################
### print output / comments for any point change
########################################################################
mOldPointStr = "Job: " + currentJob.Name() + ", " + currentJob.GetDescription();
writeln_d(" - ", mOldPointStr);
if (mOldPointStr != anOldPointStr)
{
PLATFORM.Comment(mOldPointStr);
}
return true;
}
}
}
}
return Failure("current job not a valid pursue-point job");
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing pursue-point, T=", TIME_NOW);
#PROCESSOR.SetUpdateInterval(cSLOW_UPDATE_RATE);
PLATFORM.GoToSpeed(cINTERCEPT_SPEED);
extern double cDEFAULT_ALTITUDE;
PLATFORM.GoToAltitude(cDEFAULT_ALTITUDE, 200);
if (!mCurrentPointIntercept.IsValid())
{
#PLATFORM.Comment("ignoring invalid point");
return;
}
double interceptAltitude = mCurrentPointIntercept.Altitude();
if (interceptAltitude < 0)
{
#PLATFORM.Comment("ignoring invalid point");
return;
}
double interceptRange = PLATFORM.SlantRangeTo(mCurrentPointIntercept);
double interceptHeading = PLATFORM.TrueBearingTo(mCurrentPointIntercept);
PLATFORM.TurnToHeading(interceptHeading);
writeln_d(" T=", TIME_NOW, ", range: ", interceptRange, " true-bearing: ", interceptHeading);
##if ( (interceptAltitude - PLATFORM.Altitude()) > 5)
#if ( (interceptAltitude - PLATFORM.Altitude()) > 10000*MATH.M_PER_FT())
#{
# writeln_d("GoToAltitude: ",interceptAltitude);
# #PLATFORM.GoToAltitude(interceptAltitude);
#}
end_execute
end_behavior

View File

@@ -0,0 +1,81 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pursue-point_pathing
script_variables
bool tempVariable;
WsfGeoPoint mCurrentPointIntercept;
double cSLOW_UPDATE_RATE = 3.0;
double cINTERCEPT_SPEED = 1000; # m/s
end_script_variables
precondition
writeln("precondition pursue-point_pathing");
if (!PLATFORM.GetPathFinder().IsValid())
{
writeln("no path-finder available");
return false;
}
if (PROCESSOR.GetRIPRCommanderProcessor().IsValid())
{
WsfRIPRJob currentJob = PROCESSOR.GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, PROCESSOR);
if (currentJob.IsValid())
{
// If we're supposed to fly towards a point, go for it!
if (currentJob.Name() == "pursue-point")
{
mCurrentPointIntercept = (WsfGeoPoint)currentJob.GetData("targetPoint");
if (mCurrentPointIntercept.IsValid())
{
return true;
}
}
}
}
return false;
end_precondition
execute
writeln("executing pursue-point_pathing.");
PROCESSOR.SetUpdateInterval(cSLOW_UPDATE_RATE);
PLATFORM.GoToSpeed(cINTERCEPT_SPEED);
#extern double cDEFAULT_ALTITUDE;
PLATFORM.GoToAltitude(cDEFAULT_ALTITUDE, 200);
double interceptAltitude = mCurrentPointIntercept.Altitude();
double interceptHeading = PLATFORM.TrueBearingTo(mCurrentPointIntercept);
double interceptRange = PLATFORM.SlantRangeTo(mCurrentPointIntercept);
writeln_d(" T=", TIME_NOW, ", range: ", interceptRange, " true-bearing: ", interceptHeading);
WsfGeoPoint startGeoPoint = PLATFORM.Location();
if (!PLATFORM.FindAndSetPath(startGeoPoint, mCurrentPointIntercept))
{
PLATFORM.FollowRoute("DEFAULT_ROUTE","CLOSEST_POINT");
}
#if ( (interceptAltitude - PLATFORM.Altitude()) > 5)
if ( (interceptAltitude - PLATFORM.Altitude()) > 10000*MATH.M_PER_FT())
{
writeln_d("GoToAltitude: ",interceptAltitude);
#PLATFORM.GoToAltitude(interceptAltitude);
}
end_execute
end_behavior

View File

@@ -0,0 +1,449 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pursue-target
script_debug_writes off
script_variables
//**********************************************************************//
//** debugging parameters **//
//**********************************************************************//
bool mDrawSteering = false;
//**********************************************************************//
//** control / mode of operation parameters **//
//**********************************************************************//
bool mCheckOwnJobBoardToo = false; # can this agent bid on & win a job off his own board
bool mUseMoverDuringPursuit = true;
//**********************************************************************//
//** flying parameters, for intercept or approach **//
//**********************************************************************//
//target point to fly at
WsfGeoPoint mTargetPoint = WsfGeoPoint();
double mTargetSpeed = 0; # will be overwritten
// larger values, suggested for air to air fighters & jets
double mMatchSpeedDistanceMin = 1 * 1852; # 1 mile
double mMatchSpeedDistanceMax = 20 * 1852; # 20 miles
#double mWaitSpeed = 500 * MATH.MPS_PER_NMPH();
#double mInterceptSpeed = 800 * MATH.MPS_PER_NMPH();
#double mInterceptSpeed = 293.941; //mach 0.95 at 25200 ft altitude
double mWaitSpeed = 293.941; //mach 0.95 at 25200 ft altitude
double mInterceptSpeed = 600 * MATH.MPS_PER_NMPH();
double mDefaultAccel = 7.5 * Earth.ACCEL_OF_GRAVITY(); # m/s^2 ~7.5 Gs
// smaller values, suggest for a UAV intercepting or following ground forces
#double mMatchSpeedDistanceMin = 185.2; # one tenth of a mile
#double mMatchSpeedDistanceMax = 1852.0; # a mile
#double mWaitSpeed = 22; # m/s (~50 mph)
#double mInterceptSpeed = 52; # m/s (~100 knots)
double mMinAltitude = 4572; # ~15000 feet
//switch for matching threat's altitude during pursuit
bool DefaultMatchThreatAltitude = false;
Map<string, bool> mThreatTypeMatchAltitude = Map<string, bool>();
//mThreatTypeMatchAltitude["missile_fast"] = true;
//mThreatTypeMatchAltitude["awacs"] = true;
//mThreatTypeMatchAltitude["bomber"] = true;
//mThreatTypeMatchAltitude["fighter"] = true;
mThreatTypeMatchAltitude["unknown"] = false;
mThreatTypeMatchAltitude["uav"] = false;
mThreatTypeMatchAltitude["sam"] = false;
mThreatTypeMatchAltitude["ship"] = false;
mThreatTypeMatchAltitude["jammer"] = false;
mThreatTypeMatchAltitude["missile"] = false;
//specify offset angle to fly at, during f-pole pursuit
double DefaultOffsetDistance = 1852*50; //50 nm
double DefaultOffsetAngle = 30.0; // should this be radar-specific?
Map<string, double> ThreatTypeOffsetAngle = Map<string, double>();
//ThreatTypeOffsetAngle["awacs"] = 15.0;
//ThreatTypeOffsetAngle["unknown"] = 20.0;
//ThreatTypeOffsetAngle["sam"] = 50.0;
//**********************************************************************//
//********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
//**********************************************************************//
WsfRIPRJob mCurrentJob;
WsfDraw mDraw = WsfDraw();
string mOldTargetStr = "no target";
double mLastTime = 0.0;
end_script_variables
script double GetOffsetAngleOnThreat(WsfTrack threat)
WsfPlatform plat = WsfSimulation.FindPlatform( threat.TargetName() );
if (plat.IsValid())
{
foreach( string aCategory : double angle in ThreatTypeOffsetAngle )
{
if( plat.CategoryMemberOf( aCategory ) )
{
writeln_d("offset angle for type ", aCategory, " = ", angle);
return angle;
}
}
}
return DefaultOffsetAngle;
end_script
script bool MatchAltitudeForThreat(WsfTrack track)
WsfPlatform plat = WsfSimulation.FindPlatform( track.TargetName() );
if (plat.IsValid())
{
foreach (string aCategory : bool match in mThreatTypeMatchAltitude)
{
if (plat.CategoryMemberOf(aCategory))
{
return match;
}
}
}
return DefaultMatchThreatAltitude;
end_script
precondition
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
writeln_d(PLATFORM.Name(), " precondition pursue-target, T=", TIME_NOW);
((WsfRIPRProcessor)PROCESSOR).ClearTarget();
double duration = TIME_NOW - mLastTime;
mLastTime = TIME_NOW;
if (duration > (1.5*((WsfRIPRProcessor)PROCESSOR).UpdateInterval()))
{
mOldTargetStr = "no target";
}
string anOldTargetStr = mOldTargetStr;
mOldTargetStr = "no target";
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
if (commander.IsValid())
{
if (commander.IsJobWindowOpen())
{
writeln_d("pursue-target: commander.GetJobFor()");
mCurrentJob = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
}
else
{
writeln_d("pursue-target: commander's job window closed, keeping current job!");
}
}
if (mCheckOwnJobBoardToo &&
(mCurrentJob.IsNull() || !mCurrentJob.IsValid()))
{
writeln_d("pursue-target: myself.GetJobFor()");
mCurrentJob = ((WsfRIPRProcessor)PROCESSOR).GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
}
if (mCurrentJob.IsNull() || !mCurrentJob.IsValid() )
{
writeln_d("pursue-target ClearTarget -> job not a valid pursue-target job");
return Failure("job is not a valid pursue-target job");
}
if (mCurrentJob.Name() != "pursue-target")
{
string msg = write_str("job is a ", mCurrentJob.Name(), " instead of a pursue-target job");
writeln_d(msg);
return Failure("job is not a pursue-target job");
}
#extern WsfTrack GetTrackByName (WsfPlatform, string);
string targetName = (string)mCurrentJob.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (!targetTrack.IsValid())
{
writeln_d(" No valid target track found for target named ", targetName);
return Failure("target track not found");
}
########################################################################
### print output / comments for any target change
#######################################################################
mOldTargetStr = "Job: " + mCurrentJob.Name() + ", " + mCurrentJob.GetDescription();
writeln_d(" - ", mOldTargetStr);
if (mOldTargetStr != anOldTargetStr)
{
PLATFORM.Comment(mOldTargetStr);
}
((WsfRIPRProcessor)PROCESSOR).SetTarget(targetTrack);
return true;
end_precondition
execute
string comment = write_str(PLATFORM.Name(), " executing pursue-target, T=", TIME_NOW);
writeln_d(comment);
#PLATFORM.Comment(comment);
#extern string CalculatePositioning (WsfPlatform, WsfTrack, double);
#extern double GetWeaponRangeMax(WsfPlatform aPlatform, Array<Map<string, Object>> aWeaponArray);
#extern Array<Map<string, Object>> mWeaponArray;
//get target from ripr processor, to be sure
WsfTrack targetTrack = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
if (targetTrack.IsNull() ||
!targetTrack.IsValid())
{
writeln_d(" UpdateInterceptLocation, targetTrack is null or not valid");
return;
}
if (mUseMoverDuringPursuit)
{
double ownSpeed = PLATFORM.Speed();
double targetSpeed = targetTrack.Speed();
double slantRangeTo = PLATFORM.SlantRangeTo(targetTrack);
double closingSpeed = PLATFORM.ClosingSpeedOf(targetTrack);
string positioning = CalculatePositioning(PLATFORM, targetTrack, 10.0);
int weaponsActive = ((WsfRIPRProcessor)PROCESSOR).WeaponsActive(targetTrack);
double engageRangeMax = 185200.0; //100 miles
double engageRangeMin = 1852.0; // 1 mile
# // determine the fuel needed to engage
# double fuelRequired = 0;
# double bingoQuantity = -1;
# if (mConsiderFuel)
# {
# WsfFuel fuelObj = PLATFORM.Fuel();
# if (fuelObj)
# {
# double maxWeaponRange = GetWeaponRangeMax(PLATFORM, mWeaponArray);
# double distToWeaponRange = slantRangeTo - maxWeaponRange;
# double timeToTarget = distToWeaponRange / closingSpeed;
# double distToTravel = timeToTarget * ownSpeed;
#
# fuelRequired = fuelObj.QuantityRequired(distToTravel);
# bingoQuantity = fuelObj.BingoQuantity();
# }
# }
string PursuitMode = "pure";
if (weaponsActive > 0)
{
PursuitMode = "f-pole";
}
else if (targetTrack.AirDomain())
{
if (slantRangeTo >= engageRangeMax &&
positioning != "head-to-head" &&
positioning != "head-to-tail" &&
targetSpeed >= ownSpeed)
{
PursuitMode = "lead";
}
else if (slantRangeTo <= engageRangeMax &&
positioning != "head-to-head" &&
positioning != "head-to-tail")
{
PursuitMode = "lag";
}
//else if (slantRangeTo > engageRangeMax ||
// (slantRangeTo <= engageRangeMax &&
// (positioning == "head-to-head" ||
// positioning == "head-to-tail")))
//{
// PursuitMode = "pure";
//}
}
writeln_d(" PursuitMode = ", PursuitMode);
// Our track quality (or target range) may not be good enough yet, so keep moving towards the target.
// If we got the altitude from the TRACK, match it
double interceptHeading = PLATFORM.Heading();
double distanceToTarget = PLATFORM.SlantRangeTo(targetTrack);
extern double cDEFAULT_ALTITUDE;
double interceptAltitude = cDEFAULT_ALTITUDE;
//check for targets altitude, and whether or not we should match it
if (targetTrack.ElevationValid() ||
targetTrack.LocationValid())
{
if (targetTrack.Altitude() > interceptAltitude) //always climb up to target
{
interceptAltitude = targetTrack.Altitude();
}
else if (MatchAltitudeForThreat(targetTrack) == true)
{
interceptAltitude = targetTrack.Altitude();
}
}
//always bound the altitude by the min & max restrictions (in case mover is not setup to do it)
if (interceptAltitude < mMinAltitude)
{
interceptAltitude = mMinAltitude;
}
writeln_d("desired intercept altitude: ", interceptAltitude);
mTargetSpeed = mInterceptSpeed;
if (targetTrack.VelocityValid())
{
if (targetTrack.AirDomain())
{
#extern double EffectiveRange(WsfPlatform, WsfTrack);
double speedOfTarget = targetTrack.Speed();
double effRange = EffectiveRange(PLATFORM, targetTrack);
double distanceWindow = mMatchSpeedDistanceMax - mMatchSpeedDistanceMin;
double speedWindow = mInterceptSpeed - speedOfTarget;
if(effRange < mMatchSpeedDistanceMax && effRange > mMatchSpeedDistanceMin)
{
double rangeScale = (effRange - mMatchSpeedDistanceMin) / distanceWindow;
mTargetSpeed = speedOfTarget + (speedWindow * rangeScale);
writeln_d(PLATFORM.Name(), " pursue-target, speed scaled down in matching window!");
}
else if (effRange <= mMatchSpeedDistanceMin)
{
mTargetSpeed = speedOfTarget * 0.99;
writeln_d(PLATFORM.Name(), " pursue-target, speed set to match target!");
}
if (mTargetSpeed < mWaitSpeed)
{
mTargetSpeed = mWaitSpeed;
writeln_d(PLATFORM.Name(), " pursue-target, speed was lower than wait speed, adjust!");
}
}
else if (targetTrack.LandDomain())
{
writeln_d(PLATFORM.Name(), " pursue-target, target is land domain, adjust speed!");
double speedOfTarget = targetTrack.Speed();
double range = PLATFORM.GroundRangeTo(targetTrack);
double distanceWindow = mMatchSpeedDistanceMax - mMatchSpeedDistanceMin;
double speedWindow = mInterceptSpeed - speedOfTarget;
if(range < mMatchSpeedDistanceMax && range > mMatchSpeedDistanceMin)
{
double rangeScale = (range - mMatchSpeedDistanceMin) / distanceWindow;
mTargetSpeed = speedOfTarget + (speedWindow * rangeScale);
}
else if (range <= mMatchSpeedDistanceMin)
{
mTargetSpeed = speedOfTarget * 0.99;
}
if (mTargetSpeed < mWaitSpeed)
{
mTargetSpeed = mWaitSpeed;
}
}
}
double leadOrLagTime = 15.0; //seconds
if (PursuitMode == "lead")
{
WsfWaypoint wpt = WsfWaypoint();
double tti = PLATFORM.InterceptLocation3D(targetTrack, wpt);
if (tti > 0.0)
{
mTargetPoint = wpt.Location();
}
else
{
mTargetPoint = targetTrack.LocationAtTime(TIME_NOW + leadOrLagTime);
}
}
else if(PursuitMode == "lag")
{
double usedLagDelay = (slantRangeTo/engageRangeMax) * leadOrLagTime;
double maxLagDist = 0.35 * PLATFORM.SlantRangeTo(targetTrack);
double maxLagTime = maxLagDist / targetTrack.Speed();
if (usedLagDelay > maxLagTime)
{
usedLagDelay = maxLagTime;
}
mTargetPoint = targetTrack.LocationAtTime(TIME_NOW - usedLagDelay);
}
else if (PursuitMode == "f-pole")
{
#extern double MaximizeFPole(WsfPlatform, WsfTrack, double);
interceptHeading = MaximizeFPole(PLATFORM, targetTrack, GetOffsetAngleOnThreat(targetTrack));
mTargetPoint = PLATFORM.Location();
mTargetPoint.Extrapolate(interceptHeading, DefaultOffsetDistance);
}
else
{
//PursuitMode == pure
mTargetPoint = targetTrack.LocationAtTime(TIME_NOW);
}
if (!mTargetPoint.IsValid())
{
mTargetPoint = targetTrack.CurrentLocation();
}
mTargetPoint.Set(mTargetPoint.Latitude(), mTargetPoint.Longitude(), interceptAltitude);
if (mDrawSteering == true)
{
mDraw.SetLayer("behavior_pursue_target");
mDraw.SetDuration(((WsfRIPRProcessor)PROCESSOR).UpdateInterval());
mDraw.SetColor(1.0, 0.5, 0.0);
mDraw.SetLineSize(1);
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(mTargetPoint);
mDraw.End();
}
string msg = write_str("pursue-target: ", targetTrack.TargetName(), " at speed ", (string)mTargetSpeed);
//PLATFORM.Comment(msg);
writeln_d(" T=", TIME_NOW, " ", PLATFORM.Name(), " ", msg);
#extern bool FlyTarget (WsfPlatform, WsfGeoPoint, double);
FlyTarget( PLATFORM, mTargetPoint, mTargetSpeed);
}
else
{
//go at default speed; this gets overwritten if route waypoint has defined a speed
PLATFORM.GoToSpeed(mInterceptSpeed, mDefaultAccel, true);
//return to route, at the last target route point as re-entry
#extern bool ReEnterRoute(WsfPlatform);
ReEnterRoute(PLATFORM);
if (mDrawSteering == true)
{
WsfRoute currRoute = PLATFORM.Route();
if (currRoute.IsValid())
{
mDraw.SetLayer("behavior_pursue-target");
mDraw.Erase(PLATFORM.Name());
mDraw.SetId(PLATFORM.Name());
mDraw.SetColor(0,1,1);
mDraw.SetLineSize(1);
mDraw.SetLineStyle("dash_dot2");
mDraw.BeginPolyline();
for (int i=0; i<currRoute.Size(); i=i+1)
mDraw.Vertex(currRoute.Waypoint(i).Location());
mDraw.End();
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,290 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
//pursue-target_pathing
behavior pursue-target_pathing
script_variables
double cBUCKET_BASE = 1.2;
double cINTERCEPT_SPEED = 1000; # m/s
double cWAIT_SPEED = 200; # m/s
double ownshipEngagementRangeMin = 0; // set later GetWeaponRangeMin(PLATFORM, mWeaponArray);
double ownshipEngagementRangeMax = 0; // set later GetWeaponRangeMax(PLATFORM, mWeaponArray);
#bool mUseMoverDuringPursuit = true; #set to false to force aiai to follow route during pursuit of target
//use this offset angle if a category specific offset isn't found
double mFPoleAngle = 45.0; // this should be radar-specific
//fly different offset angles, depending on the type or category of threat
Map<string, double> ThreatTypeOffsetAngle = Map<string, double>();
ThreatTypeOffsetAngle["unknown"] = 45.0;
ThreatTypeOffsetAngle["sam"] = 50.0;
ThreatTypeOffsetAngle["awacs"] = 15.0;
ThreatTypeOffsetAngle["fighter"] = 35.0;
end_script_variables
precondition
writeln("precondition pursue-target_pathing");
#extern WsfTrack GetTrackByName (WsfPlatform, string);
#extern bool TestTrackCategory (WsfTrack, string);
if (!PLATFORM.GetPathFinder().IsValid())
{
writeln("no path-finder available");
return false;
}
// Clear the variables we'll use to store the agent's output
PROCESSOR.ClearTarget();
WsfRIPRJob currentJob = null;
if (PROCESSOR.GetRIPRCommanderProcessor().IsValid())
{
currentJob = PROCESSOR.GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, PROCESSOR);
}
if (!currentJob.IsValid() || currentJob.Name() != "pursue-target")
{
writeln_d(" job not valid pursue-target job from board!");
return false;
}
string targetName = (string)currentJob.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (!targetTrack.IsValid() || TestTrackCategory(targetTrack, "unknown"))
{
writeln_d(" No known category track found for target ", targetName);
return false;
}
PROCESSOR.SetTarget(targetTrack);
return true;
end_precondition
on_init
#extern double GetWeaponRangeMax (WsfPlatform, Array<Map<string, Object>>);
#extern double GetWeaponRangeMin (WsfPlatform, Array<Map<string, Object>>);
#extern Array<Map<string, Object>> mWeaponArray;
ownshipEngagementRangeMin = GetWeaponRangeMin(PLATFORM, mWeaponArray);
ownshipEngagementRangeMax = GetWeaponRangeMax(PLATFORM, mWeaponArray);
end_on_init
script double GetOffsetAngleOnThreat(WsfTrack threat)
WsfPlatform plat = WsfSimulation.FindPlatform( threat.TargetName() );
if (plat.IsValid())
{
foreach( string aCategory : double angle in ThreatTypeOffsetAngle )
{
if( plat.CategoryMemberOf( aCategory ) )
{
writeln_d("offset angle for type ", aCategory, " = ", angle);
return angle;
}
}
}
return mFPoleAngle;
end_script
execute
writeln("executing pursue-target_pathing.");
#extern int GetBucket (double, double);
#extern string CalculatePositioning (WsfPlatform, WsfTrack, double);
//get target from ripr processor, to be sure
WsfTrack targetTrack = PROCESSOR.GetTarget();
double ownSpeed = GetBucket(PLATFORM.Speed(), cBUCKET_BASE);
double engageRangeMin = GetBucket(ownshipEngagementRangeMin, cBUCKET_BASE);
double engageRangeMax = GetBucket(ownshipEngagementRangeMax, cBUCKET_BASE);
double slantRangeTo = GetBucket(PLATFORM.SlantRangeTo(targetTrack), cBUCKET_BASE);
double targetSpeed = GetBucket(targetTrack.Speed(), cBUCKET_BASE);
string positioning = CalculatePositioning(PLATFORM, targetTrack, GetOffsetAngleOnThreat(targetTrack));
int weaponsActive = PROCESSOR.WeaponsActive(targetTrack);
string mPursuitMode = "pure";
if (weaponsActive > 0)
{
mPursuitMode = "f-pole";
}
else if (targetTrack.AirDomain())
{
if (slantRangeTo >= engageRangeMax &&
positioning != "head-to-head" &&
positioning != "head-to-tail" &&
targetSpeed >= ownSpeed)
{
mPursuitMode = "lead";
}
else if (slantRangeTo <= engageRangeMax &&
positioning != "head-to-head" &&
positioning != "head-to-tail")
{
mPursuitMode = "lag";
}
else if (slantRangeTo > engageRangeMax ||
(slantRangeTo <= engageRangeMax &&
(positioning == "head-to-head" ||
positioning == "head-to-tail")))
{
mPursuitMode = "pure";
}
}
#TransitionToINTERCEPT( targetTrack, mPursuitMode );
#PLATFORM.GoToSpeed(cINTERCEPT_SPEED);
#if (mUseMoverDuringPursuit)
#{
# #nothing
# PLATFORM.GoToAltitude(cDEFAULT_ALTITUDE, 200);
#}
#else
#{
# #TransitionToWAIT(true); #FAIL
# #PLATFORM.FollowRoute("DEFAULT_ROUTE", "CLOSEST_POINT"); #better
# PLATFORM.ReturnToRoute();
# PLATFORM.GoToAltitude(cDEFAULT_ALTITUDE, 200, true);
#}
#PROCESSOR.SetUpdateInterval(cFAST_UPDATE_RATE);
#writeln_d(" Transition to INTERCEPT due to pursuing ", targetTrack.TargetName(), " mode ", mPursuitMode);
#end_script
#CheckAndFire(targetTrack); #do this in the weapon management behavior???
// Our track quality (or target range) may not be good enough yet, so keep moving towards the target.
if (targetTrack.IsNull())
{
writeln_d("behavior pursue-target_pathing, targetTrack is null");
return;
}
writeln_d(" ", mPursuitMode, " pursuit");
// If we got the altitude from the TRACK, match it
#extern double cDEFAULT_ALTITUDE;
double interceptAltitude = cDEFAULT_ALTITUDE;
//double interceptAltitude = PLATFORM.Altitude();
double interceptHeading = PLATFORM.Heading();
double distanceToTarget = PLATFORM.SlantRangeTo(targetTrack);
bool useIntercept = false;
double desiredSpeed = cINTERCEPT_SPEED;
if (targetTrack.VelocityValid() && targetTrack.AirDomain())
{
#extern double EffectiveRange(WsfPlatform, WsfTrack);
#extern double GetWeaponRangeMax (WsfPlatform, Array<Map<string, Object>>);
#extern double GetWeaponRangeMin (WsfPlatform, Array<Map<string, Object>>);
double rangeMin = GetWeaponRangeMin(PLATFORM, mWeaponArray);
double rangeMax = GetWeaponRangeMax(PLATFORM, mWeaponArray);
double speedOfTarget = targetTrack.Speed();
double effRange = EffectiveRange(PLATFORM, targetTrack);
double missileWindow = rangeMax - rangeMin;
double speedWindow = cINTERCEPT_SPEED - speedOfTarget;
if(effRange < rangeMax && effRange > rangeMin)
{
double rangeScale = (effRange - rangeMin) / missileWindow;
desiredSpeed = speedOfTarget + (speedWindow * rangeScale);
}
else if (effRange <= rangeMin)
{
desiredSpeed = speedOfTarget;
}
if (desiredSpeed < cWAIT_SPEED)
{
desiredSpeed = cWAIT_SPEED;
}
}
PLATFORM.GoToSpeed(desiredSpeed);
WsfGeoPoint startGeoPoint = PLATFORM.Location();
WsfGeoPoint endGeoPoint = targetTrack.ReportedLocation();
if (mPursuitMode == "lead")
{
double timeToIntercept = 0.0;
WsfWaypoint interceptPoint = WsfWaypoint();
timeToIntercept = PLATFORM.InterceptLocation3D(targetTrack, interceptPoint);
// If timeToIntercept is positive then we know intercept is possible
if (timeToIntercept > 0.0)
{
endGeoPoint = targetTrack.ReportedLocation();
#writeln_d(" T=", TIME_NOW, " TTI=", timeToIntercept, " range: ", PLATFORM.SlantRangeTo(targetTrack), " true-bearing: ", interceptPoint.Heading());
#writeln_d(" lead pursuit: ", interceptPoint.Latitude(), ", ", interceptPoint.Longitude(), ", ", interceptPoint.Altitude());
}
else
{
writeln_d(" lead pursuit attempt with timeToIntercept < 0");
}
}
else if (mPursuitMode == "f-pole")
{
#extern double MaximizeFPole(WsfPlatform, WsfTrack, double);
interceptHeading = MaximizeFPole(PLATFORM, targetTrack, GetOffsetAngleOnThreat(targetTrack));
endGeoPoint = PLATFORM.Location();
//f-pole towards a point 30 miles away in the correct direction
endGeoPoint.Extrapolate(interceptHeading, 55560);
#writeln_d(" T=", TIME_NOW, ", range: ", PLATFORM.SlantRangeTo(targetTrack), " true-bearing: ", PLATFORM.TrueBearingTo(targetTrack));
}
else
{
//otherwise... just fly at the target
endGeoPoint = targetTrack.ReportedLocation();
#writeln_d(" T=", TIME_NOW, ", range: ", PLATFORM.SlantRangeTo(targetTrack), " true-bearing: ", PLATFORM.TrueBearingTo(targetTrack),
# " (", endGeoPoint.Latitude(), "/", targetTrack.Latitude(), ",", endGeoPoint.Longitude(), "/", targetTrack.Longitude(), ")");
}
//make sure we don't dive, staying above the target is fine
if (endGeoPoint.Altitude() < interceptAltitude)
{
endGeoPoint.Set(endGeoPoint.Latitude(), endGeoPoint.Longitude(), interceptAltitude);
}
if (!PLATFORM.FindAndSetPath(startGeoPoint, endGeoPoint))
{
PLATFORM.FollowRoute("DEFAULT_ROUTE","CLOSEST_POINT");
#PLATFORM.Comment("Route: DEFAULT");
}
end_execute
end_behavior

View File

@@ -0,0 +1,105 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pursue_altitude
script_debug_writes off
script_variables
WsfTrack mTargetTrack;
double mMinAltitude = 4572; # ~15000 feet
//switch for matching threat's altitude during pursuit
bool DefaultMatchThreatAltitude = false;
Map<string, bool> mThreatTypeMatchAltitude = Map<string, bool>();
mThreatTypeMatchAltitude["unknown"] = false;
mThreatTypeMatchAltitude["uav"] = false;
mThreatTypeMatchAltitude["sam"] = false;
mThreatTypeMatchAltitude["ship"] = false;
mThreatTypeMatchAltitude["jammer"] = false;
mThreatTypeMatchAltitude["missile"] = false;
//mThreatTypeMatchAltitude["missile_fast"] = true;
//mThreatTypeMatchAltitude["awacs"] = true;
//mThreatTypeMatchAltitude["bomber"] = true;
//mThreatTypeMatchAltitude["fighter"] = true;
end_script_variables
script bool MatchAltitudeForThreat(WsfTrack track)
WsfPlatform plat = WsfSimulation.FindPlatform( track.TargetName() );
if (plat.IsValid())
{
foreach (string aCategory : bool match in mThreatTypeMatchAltitude)
{
if (plat.CategoryMemberOf(aCategory))
{
return match;
}
}
}
return DefaultMatchThreatAltitude;
end_script
precondition
writeln_d(PLATFORM.Name(), " precondition pursue_altitude, T=", TIME_NOW);
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
//get target from ripr processor, to be sure
mTargetTrack = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
if (mTargetTrack.IsNull() || !mTargetTrack.IsValid())
{
writeln_d(" No valid target track found to pursue!");
return Failure("target track not found");
}
return true;
end_precondition
execute
string comment = write_str(PLATFORM.Name(), " executing pursue_altitude, T=", TIME_NOW);
writeln_d(comment);
#PLATFORM.Comment(comment);
extern double mDesiredAltitude;
// If we got the altitude from the TRACK, match it
extern double cDEFAULT_ALTITUDE;
mDesiredAltitude = cDEFAULT_ALTITUDE;
//check for targets altitude, and whether or not we should match it
if (mTargetTrack.ElevationValid() ||
mTargetTrack.LocationValid())
{
if (mTargetTrack.Altitude() > mDesiredAltitude) //always climb up to target
{
mDesiredAltitude = mTargetTrack.Altitude();
}
else if (MatchAltitudeForThreat(mTargetTrack) == true) //just useful for diving down to threat lower level
{
mDesiredAltitude = mTargetTrack.Altitude();
}
}
//always bound the altitude by the min & max restrictions (in case mover is not setup to do it)
if (mDesiredAltitude < mMinAltitude)
{
mDesiredAltitude = mMinAltitude;
}
writeln_d("desired intercept altitude: ", mDesiredAltitude);
end_execute
end_behavior

View File

@@ -0,0 +1,232 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
##
## purse_base, aggregate behavior
## this behavior owns the bidding block for pursue-target jobs and acquires the target
## this behavior owns the [main] precondition block for this set of behaviors
## this behavior owns the variables for heading, alt, speed, but child behaviors calculate them
## last child behavior should actually do the flying
##
include_once behavior_pursue_heading.txt //original ripr intercept logic
#include_once behavior_pursue_heading_smooth.txt //option: smooth turning intercept
include_once behavior_pursue_altitude.txt
include_once behavior_pursue_speed.txt
include_once behavior_pursue_fly_simple.txt //original ripr flight command
#include_once behavior_pursue_fly_route_finder.txt //option: fly around avoidances using WsfRouteFinder
behavior pursue_base
script_debug_writes off
script_variables
//**********************************************************************//
//** aggregate parameters (modifeid by or used by child behaviors) **//
//**********************************************************************//
//these three are modified by child behaviors, not edited here
double mDesiredHeading;
double mDesiredAltitude;
double mDesiredSpeed;
//user can edit these three
double mDesiredLinearAccel = 7.5 * Earth.ACCEL_OF_GRAVITY(); // 7.5 G (m/s^2)
double mDesiredRadialAccel = 6.0 * Earth.ACCEL_OF_GRAVITY(); // 6.0 G (m/s^2) //default max limit for air mover
double mDesiredClimbDiveAngle = 35; //(degrees) //should this be climb/dive rate instead of angle?
//**********************************************************************//
//** bidding parameters (pursue-target jobs) **//
//** slant range and closing speed are the most important **//
//** bids are now in units of distance (meters) **//
//** all other bid contributions are converted with their weight **//
//** range will dominate the bids **//
//**********************************************************************//
double cMIN_JOB_BID = -MATH.DOUBLE_MAX();
double cMAX_SLANT_RANGE = 1000000.0; #over 539 nm away, target ranges beyond this are considered unfavorable
double cWEIGHT_SLANT_RANGE_TO = 1.0;
double cMIN_CLOSING_SPEED = -1050.0; #threats running away (negative closing) faster are considered unfavorable
double cWEIGHT_CLOSING_SPEED = 1.0; #scale for how closing speed translates to distance
double cWEIGHT_FUEL = 0.0; #try a value of 2.0 if you care about fuel
double cWEIGHT_MY_WEAPONS_ACTIVE = 0.0; #changes bid if your own weapons are active on target
double cWEIGHT_PEERS_WEAPONS_ACTIVE = 0.0; #changes bid if your peers weapons are active on target
double cWEIGHT_THREAT_WEAPONS_ENVELOPE = 0.0; #uses the global "mWeaponsEnvelope" array, try value of 10000 if you care about that
//careful with these two parameters, they are stateful
double cWEIGHT_CURRENT_TARGET = 0.0; #changes bid if you are currently targeting the threat
double cWEIGHT_OTHERS_TARGETING = 0.0; #changes bid if peers are currently targeting the threat
//**********************************************************************//
//** control / mode of operation parameters **//
//**********************************************************************//
#bool mFilterJobsOnWeapons = true; # ignore pursue-target jobs that we cant shoot a weapon at
bool mFilterJobsOnWeapons = false; # ignore pursue-target jobs that we cant shoot a weapon at
bool mFilterJobsOnCategory = false; # ignore pursue-target jobs that are for platform of unknown category
bool mFilterJobsOnFuel = false; # ignore pursue-target jobs that we dont have the fuel to reach
bool mFilterJobsOnZone = false; # ignore pursue-target jobs that are for platforms outsize of zone "mZoneName"
string mZoneName = "";
WsfZone mFezZone;
bool mCheckOwnJobBoardToo = false; # can this agent bid on & win a job off his own board
//**********************************************************************//
//** debugging parameters **//
//**********************************************************************//
#bool mDrawSteering = false;
//**********************************************************************//
//********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
//**********************************************************************//
WsfRIPRJob mCurrentJob;
#WsfDraw mDraw = WsfDraw();
string mOldTargetStr = "no target";
double mLastTime = 0.0;
end_script_variables
script bool HaveWeaponsForThreat(WsfTrack track)
#extern bool IsWeaponDomainCapable(WsfTrack, Map<string, Object>);
extern Array<Map<string, Object>> mWeaponArray;
foreach (Map<string, Object> curWeapon in mWeaponArray)
{
WsfWeapon weapon = (WsfWeapon)curWeapon["weapon"];
if (weapon.QuantityRemaining() > 0 &&
IsWeaponDomainCapable(track,curWeapon))
{
if (curWeapon.Exists("onlyUseInRange") &&
(int)curWeapon["onlyUseInRange"] == 1 &&
PLATFORM.SlantRangeTo(track) > (double)curWeapon["rangeMax"] )
{
continue;
}
return true;
}
}
return false;
end_script
precondition
writeln_d(PLATFORM.Name(), " precondition pursue-target, T=", TIME_NOW);
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
double duration = TIME_NOW - mLastTime;
mLastTime = TIME_NOW;
if (duration > (1.5*PROCESSOR.UpdateInterval()))
{
mOldTargetStr = "no target";
}
string anOldTargetStr = mOldTargetStr;
mOldTargetStr = "no target";
WsfRIPRProcessor commander = ((WsfRIPRProcessor)PROCESSOR).GetRIPRCommanderProcessor();
if (commander.IsValid())
{
if (commander.IsJobWindowOpen())
{
writeln_d("pursue-target: commander.GetJobFor()");
mCurrentJob = commander.GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
}
else
{
writeln_d("pursue-target: commander's job window closed, keeping current job!");
}
}
if (mCheckOwnJobBoardToo &&
(mCurrentJob.IsNull() || !mCurrentJob.IsValid()))
{
writeln_d("pursue-target: myself.GetJobFor()");
mCurrentJob = ((WsfRIPRProcessor)PROCESSOR).GetJobFor(TIME_NOW, ((WsfRIPRProcessor)PROCESSOR));
}
if (mCurrentJob.IsNull() || !mCurrentJob.IsValid() )
{
writeln_d("pursue-target ClearTarget -> job not a valid pursue-target job");
((WsfRIPRProcessor)PROCESSOR).ClearTarget();
return Failure("job is not a valid pursue-target job");
}
if (mCurrentJob.Name() != "pursue-target")
{
string msg = write_str("job is a ", mCurrentJob.Name(), " instead of a pursue-target job");
writeln_d(msg);
return Failure("job is not a pursue-target job");
}
#extern WsfTrack GetTrackByName (WsfPlatform, string);
string targetName = (string)mCurrentJob.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (!targetTrack.IsValid())
{
writeln_d(" No valid target track found for target named ", targetName);
return Failure("target track not found");
}
########################################################################
### print output / comments for any target change
#######################################################################
mOldTargetStr = "Job: " + mCurrentJob.Name() + ", " + mCurrentJob.GetDescription();
writeln_d(" - ", mOldTargetStr);
if (mOldTargetStr != anOldTargetStr)
{
PLATFORM.Comment(mOldTargetStr);
}
((WsfRIPRProcessor)PROCESSOR).SetTarget(targetTrack);
return true;
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing pursue_base, T=", TIME_NOW);
# if (mDrawSteering == true)
# {
# mDraw.SetLayer("behavior_pursue_target");
# mDraw.SetDuration(PROCESSOR.UpdateInterval());
# mDraw.SetColor(1.0, 0.5, 0.0);
# mDraw.SetLineSize(1);
# mDraw.BeginLines();
# mDraw.Vertex(PLATFORM.Location());
# mDraw.Vertex(mTargetPoint);
# mDraw.End();
# }
end_execute
parallel
precondition
WsfTrack target = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
if (target.IsNull() || !target.IsValid()) {
return Failure("target track not found");
}
if (!target.AirDomain()) {
#writeln("track is not for an air-domain target");
return Failure("track is not for an air-domain target");
}
return true;
end_precondition
behavior_node pursue_heading
behavior_node pursue_altitude
behavior_node pursue_speed
behavior_node pursue_fly_simple
end_parallel
end_behavior

View File

@@ -0,0 +1,185 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# THIS BEHAVIOR ASSUMES THE FOLLOWING VARIABLES EXIST AND ARE SET
# #extern double mDesiredHeading;
# #extern double mDesiredAltitude;
# #extern double mDesiredSpeed;
behavior pursue_fly_route_finder
script_debug_writes off
script_variables
//expected global#externs
#extern Array<WsfGeoPoint> gAvoidPoints;
#extern Array<double> gAvoidRadii;
WsfRouteFinder mRouteFinder = WsfRouteFinder();
bool mDebugDraw = true;
WsfGeoPoint mTargetPoint;
WsfDraw mDraw = WsfDraw();
bool mIgnoreHeadingFindMyOwn = false; // if true: find own heading towards target track
bool mUseGlobalAvoids = false; //true: use#extern gAvoidPoints and gAvoidRadii
//false: use these two arrays below
Array<WsfGeoPoint> mAvoidPoints = Array<WsfGeoPoint>();
Array<double> mAvoidRadii = Array<double>();
end_script_variables
on_init
mDraw.SetLayer("pursue-target_route_finder");
mDraw.SetLineSize(1);
#mRouteFinder.SetImpossibleRouteResponse(2); //shift starting or ending point outside of any avoidances (dont shrink the avoidance regions)
if (mUseGlobalAvoids == true)
{
#extern Array<WsfGeoPoint> gAvoidPoints;
#extern Array<double> gAvoidRadii;
for (int i=0; i < gAvoidPoints.Size() && i < gAvoidRadii.Size(); i=i+1)
{
WsfGeoPoint pt = gAvoidPoints[i];
double radius = gAvoidRadii[i];
writeln_d(PLATFORM.Name(), " avoiding: ", pt.ToString(), ", at radius: ", radius);
mRouteFinder.Avoid(pt, radius);
}
}
else
{
for (int i=0; i < mAvoidPoints.Size() && i < mAvoidRadii.Size(); i=i+1)
{
WsfGeoPoint pt = mAvoidPoints[i];
double radius = mAvoidRadii[i];
writeln_d(PLATFORM.Name(), " avoiding: ", pt.ToString(), ", at radius: ", radius);
mRouteFinder.Avoid(pt, radius);
}
}
end_on_init
precondition
writeln_d(PLATFORM.Name(), " precondition pursue_fly_route_finder, T=", TIME_NOW);
if (mIgnoreHeadingFindMyOwn == true)
{
WsfTrack targetTrack = PROCESSOR.GetTarget();
if (targetTrack.IsNull() || !targetTrack.IsValid())
{
writeln_d(" No valid target track found to pursue!");
return Failure("target track not found");
}
mTargetPoint = targetTrack.CurrentLocation();
}
else
{
//extrapolate desired heading foward far enough to account for our speed & turning ability
#extern double mDesiredHeading;
double dist = 1852*10; //calculate based on speed & turning
mTargetPoint = PLATFORM.Location();
mTargetPoint.Extrapolate(mDesiredHeading, dist);
}
return true;
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing pursue_fly_route_finder, T=", TIME_NOW);
#extern double mDesiredHeading;
#extern double mDesiredAltitude;
#extern double mDesiredSpeed;
#extern double mDesiredLinearAccel;
#extern double mDesiredRadialAccel; #this will be ignored, the route finder gives us this
#extern double mDesiredClimbDiveAngle;
# PLATFORM.GoToSpeed(mDesiredSpeed, mDesiredLinearAccel);
# double altRateOfChange = PLATFORM.Speed() * MATH.Sin(mDesiredClimbDiveAngle);
# PLATFORM.GoToAltitude(mDesiredAltitude, altRateOfChange);
# PLATFORM.TurnToHeading(mDesiredHeading, mDesiredRadialAccel);
if (PLATFORM.SlantRangeTo(mTargetPoint) > (3*mDesiredSpeed)) // if we are more than 2 seconds away from our target
{
#mRouteFinder.SetImpossibleRouteResponse(3); //dont shrink any avoidances or shift any points
mRouteFinder.SetImpossibleRouteResponse(2); //shift points to exist outside of avoidances
WsfRoute path = mRouteFinder.Route(TIME_NOW, PLATFORM.Location(), mTargetPoint, mDesiredSpeed);
if (!path.IsValid())
{
writeln_d("***** ERROR: INVALID PATH!!!");
return;
}
WsfRoute avoidances = mRouteFinder.RouteAvoidances();
if (!avoidances.IsValid())
{
writeln_d("***** ERROR: INVALID AVOIDANCES!!!");
return;
}
writeln_d("T=", TIME_NOW, ", path size: ", path.Size(), ", avoidances: ", avoidances.Size());
if (path.Size() >= 2 && avoidances.Size() >= 1)
{
double speed = path[0].Speed();
double radAccel;
double heading;
double avoidAngle = 90;
if (PLATFORM.RelativeBearingTo(avoidances[0].Location()) < 0)
{
avoidAngle = -90;
}
if (path[0].Location().SlantRangeTo(path[1].Location()) < (speed*3))
{
//just stay orthogonal to avoidance, we are riding the edge of the circle now
heading = PLATFORM.TrueBearingTo(avoidances[0].Location()) - avoidAngle;
radAccel = path[0].RadialAcceleration();
}
else
{
//we are on a leg of the route, not on a circle right now
//just turn towards the next point
heading = PLATFORM.TrueBearingTo(path[1].Location());
radAccel = 100; //incredible radial acceleration, let mover limit it
//this is to get platform oriented onto straight leg of the route
}
writeln_d(PLATFORM.Name(), ", rad accel: ", radAccel, ", speed: ", speed, ", loc: ", path[1].Location().ToString());
PLATFORM.TurnToHeading( heading, radAccel);
PLATFORM.GoToSpeed( path[0].Speed(), path[0].LinearAcceleration(), true);
if (mDebugDraw == true)
{
//draw white line on avoidance side, length = radius of avoidance
double radAccel = path[0].RadialAcceleration();
double speed = path[0].Speed();
double radius = speed * speed / radAccel;
WsfGeoPoint center = PLATFORM.Location();
center.Extrapolate(PLATFORM.Heading()+avoidAngle, radius);
mDraw.SetDuration(1);
mDraw.SetColor(1.0, 1.0, 1.0); //white
mDraw.BeginLines();
mDraw.Vertex(PLATFORM.Location());
mDraw.Vertex(center);
mDraw.End();
}
}
}
if (mDebugDraw == true)
{
mRouteFinder.DrawRoute(PROCESSOR.UpdateInterval(), Vec3.Construct(0.0, 0.75, 0.0)); #green
mRouteFinder.DrawAvoidances(PROCESSOR.UpdateInterval(), Vec3.Construct(0.5, 0.5, 0.5)); #gray
}
end_execute
end_behavior

View File

@@ -0,0 +1,44 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# THIS BEHAVIOR ASSUMES THE FOLLOWING VARIABLES EXIST AND ARE SET
# #extern double mDesiredHeading;
# #extern double mDesiredAltitude;
# #extern double mDesiredSpeed;
behavior pursue_fly_simple
script_debug_writes off
#script_variables
#end_script_variables
precondition
writeln_d(PLATFORM.Name(), " precondition pursue_fly_simple, T=", TIME_NOW);
return true;
end_precondition
execute
writeln_d(PLATFORM.Name(), " executing pursue_fly_simple, T=", TIME_NOW);
extern double mDesiredHeading;
extern double mDesiredAltitude;
extern double mDesiredSpeed;
extern double mDesiredLinearAccel;
extern double mDesiredRadialAccel;
extern double mDesiredClimbDiveAngle;
writeln_d("flying heading: ", mDesiredHeading, ", alt: ", mDesiredAltitude, ", speed: ", mDesiredSpeed);
PLATFORM.GoToSpeed(mDesiredSpeed, mDesiredLinearAccel);
double altRateOfChange = PLATFORM.Speed() * MATH.Sin(mDesiredClimbDiveAngle);
PLATFORM.GoToAltitude(mDesiredAltitude, altRateOfChange);
PLATFORM.TurnToHeading(mDesiredHeading, mDesiredRadialAccel);
end_execute
end_behavior

View File

@@ -0,0 +1,141 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pursue_heading
script_debug_writes off
script_variables
WsfTrack mTargetTrack;
//specify offset angle to fly at, during f-pole pursuit
double DefaultOffsetAngle = 30.0; // should this be radar-specific?
Map<string, double> ThreatTypeOffsetAngle = Map<string, double>();
#ThreatTypeOffsetAngle["awacs"] = 15.0;
#ThreatTypeOffsetAngle["unknown"] = 20.0;
#ThreatTypeOffsetAngle["sam"] = 50.0;
end_script_variables
script double GetOffsetAngleOnThreat(WsfTrack threat)
WsfPlatform plat = WsfSimulation.FindPlatform( threat.TargetName() );
if (plat.IsValid())
{
foreach( string aCategory : double angle in ThreatTypeOffsetAngle )
{
if( plat.CategoryMemberOf( aCategory ) )
{
writeln_d("offset angle for type ", aCategory, " = ", angle);
return angle;
}
}
}
return DefaultOffsetAngle;
end_script
precondition
writeln_d(PLATFORM.Name(), " precondition pursue_heading, T=", TIME_NOW);
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
//get target from ripr processor, to be sure
mTargetTrack = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
if (mTargetTrack.IsNull() || !mTargetTrack.IsValid())
{
writeln_d(" No valid target track found to pursue!");
return Failure("target track not found");
}
return true;
end_precondition
execute
string comment = write_str(PLATFORM.Name(), " executing pursue_heading, T=", TIME_NOW);
writeln_d(comment);
#PLATFORM.Comment(comment);
extern double mDesiredHeading;
#extern string CalculatePositioning(WsfPlatform, WsfTrack, double);
double slantRangeTo = PLATFORM.SlantRangeTo(mTargetTrack);
string positioning = CalculatePositioning(PLATFORM, mTargetTrack, 10.0);
double engageRangeMax = 185200.0; //100 miles
double engageRangeMin = 1852.0; // 1 mile
string PursuitMode = "pure";
if (((WsfRIPRProcessor)PROCESSOR).WeaponsActive(mTargetTrack) > 0)
{
PursuitMode = "f-pole";
}
else if (mTargetTrack.AirDomain())
{
if (slantRangeTo >= engageRangeMax &&
positioning != "head-to-head" &&
positioning != "head-to-tail" &&
mTargetTrack.Speed() >= PLATFORM.Speed())
{
PursuitMode = "lead";
}
else if (slantRangeTo <= engageRangeMax &&
positioning != "head-to-head" &&
positioning != "head-to-tail")
{
PursuitMode = "lag";
}
}
writeln_d(" PursuitMode = ", PursuitMode);
mDesiredHeading = PLATFORM.Heading();
double leadOrLagTime = 15.0; //seconds
if (PursuitMode == "lead")
{
WsfWaypoint wpt = WsfWaypoint();
double tti = PLATFORM.InterceptLocation3D(mTargetTrack, wpt);
if (tti > 0.0)
{
mDesiredHeading = PLATFORM.TrueBearingTo(wpt.Location());
}
else
{
mDesiredHeading = PLATFORM.TrueBearingTo(mTargetTrack.LocationAtTime(TIME_NOW + leadOrLagTime));
}
}
else if(PursuitMode == "lag")
{
double usedLagDelay = (slantRangeTo/engageRangeMax) * leadOrLagTime;
double maxLagDist = 0.35 * slantRangeTo;
double maxLagTime = maxLagDist / mTargetTrack.Speed();
if (usedLagDelay > maxLagTime)
{
usedLagDelay = maxLagTime;
}
mDesiredHeading = PLATFORM.TrueBearingTo(mTargetTrack.LocationAtTime(TIME_NOW - usedLagDelay));
}
else if (PursuitMode == "f-pole")
{
#extern double MaximizeFPole(WsfPlatform, WsfTrack, double);
mDesiredHeading = MaximizeFPole(PLATFORM, mTargetTrack, GetOffsetAngleOnThreat(mTargetTrack));
}
else
{
//PursuitMode == pure
mDesiredHeading = PLATFORM.TrueBearingTo(mTargetTrack.CurrentLocation());
}
end_execute
end_behavior

View File

@@ -0,0 +1,88 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pursue_heading_smooth
script_debug_writes off
script_variables
//parameters for user edit
double mInterceptRevectorThreshold = 5.0; # percent of range
double mInterceptLargeHeadingChange = 10.0; # degrees
//private
WsfTrack mTargetTrack;
WsfGeoPoint mInterceptLoc = WsfGeoPoint();
end_script_variables
precondition
writeln_d(PLATFORM.Name(), " precondition pursue_heading_smooth, T=", TIME_NOW);
//get target from ripr processor, to be sure
mTargetTrack = PROCESSOR.GetTarget();
if (mTargetTrack.IsNull() || !mTargetTrack.IsValid())
{
writeln_d(" No valid target track found to pursue!");
return Failure("target track not found");
}
return true;
end_precondition
execute
string comment = write_str(PLATFORM.Name(), " executing pursue_heading_smooth, T=", TIME_NOW);
writeln_d(comment);
#PLATFORM.Comment(comment);
#extern double mDesiredHeading; //for read/write
#extern double mDesiredRadialAccel; //for read/write
#extern double mDesiredSpeed; //for read only here
// The intercept point may fluctuate because of manuevers or track errors.
// Minor changes in the intercept point are ignored.
WsfGeoPoint newInterceptLoc = mTargetTrack.CurrentLocation();
double interceptDelta = newInterceptLoc.SlantRangeTo(mInterceptLoc);
double revectorThreshold = mInterceptRevectorThreshold * 0.01 * PLATFORM.SlantRangeTo(newInterceptLoc);
if (interceptDelta > revectorThreshold)
{
mInterceptLoc.Set(newInterceptLoc);
}
// The standard air mover will turn at the current maximum allowable rate to make the
// heading change. This causes a lot of wing-wobble that wouldn't appear in real life.
// Compute the minimum acceleration needed to complete the turn well before predicted intercept
double headingChange = PLATFORM.RelativeBearingTo(mInterceptLoc);
double absHeadingChange = MATH.Fabs(headingChange);
double interceptRange = PLATFORM.SlantRangeTo(mInterceptLoc);
double timeToIntercept = interceptRange / mDesiredSpeed;
double minRadialAccel = (absHeadingChange / (0.5 * timeToIntercept)) * mDesiredSpeed;
if (minRadialAccel < 0.01) minRadialAccel = 0.01;
// Major heading changes (greater than mInterceptLargeHeadingChange degrees) are performed
// at 1g. Minor heading changes are performed at successively smaller rates. Both are
// subject to the minimum computed above.
double radialAccel = 9.8;
if (absHeadingChange < mInterceptLargeHeadingChange)
{
radialAccel = 9.8 * (absHeadingChange * absHeadingChange) /
(mInterceptLargeHeadingChange * mInterceptLargeHeadingChange);
}
if (radialAccel < minRadialAccel)
{
radialAccel = minRadialAccel;
}
//set the values of the required parameters when calculated
mDesiredHeading = PLATFORM.TrueBearingTo(mInterceptLoc);
mDesiredRadialAccel = radialAccel;
end_execute
end_behavior

View File

@@ -0,0 +1,112 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior pursue_speed
script_debug_writes off
script_variables
WsfTrack mTargetTrack;
// these values suggested for air to air fighters & jets
double mMatchSpeedDistanceMin = 1 * 1852; # 1 mile
double mMatchSpeedDistanceMax = 20 * 1852; # 20 miles
double mWaitSpeed = 293.941; //mach 0.95 at 25200 ft altitude
double mInterceptSpeed = 600 * MATH.MPS_PER_NMPH();
#double mWaitSpeed = 500 * MATH.MPS_PER_NMPH();
#double mInterceptSpeed = 800 * MATH.MPS_PER_NMPH();
#double mInterceptSpeed = 293.941; //mach 0.95 at 25200 ft altitude
end_script_variables
precondition
writeln_d(PLATFORM.Name(), " precondition pursue_speed, T=", TIME_NOW);
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
} // ((WsfRIPRProcessor)PROCESSOR)
//get target from ripr processor, to be sure
mTargetTrack = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
if (mTargetTrack.IsNull() || !mTargetTrack.IsValid())
{
writeln_d(" No valid target track found to pursue!");
return Failure("target track not found");
}
return true;
end_precondition
execute
string comment = write_str(PLATFORM.Name(), " executing pursue_speed, T=", TIME_NOW);
writeln_d(comment);
#PLATFORM.Comment(comment);
extern double mDesiredSpeed;
mDesiredSpeed = mInterceptSpeed;
if (mTargetTrack.VelocityValid())
{
if (mTargetTrack.AirDomain())
{
#extern double EffectiveRange(WsfPlatform, WsfTrack);
double speedOfTarget = mTargetTrack.Speed();
double effRange = EffectiveRange(PLATFORM, mTargetTrack);
double distanceWindow = mMatchSpeedDistanceMax - mMatchSpeedDistanceMin;
double speedWindow = mInterceptSpeed - speedOfTarget;
if(effRange < mMatchSpeedDistanceMax && effRange > mMatchSpeedDistanceMin)
{
double rangeScale = (effRange - mMatchSpeedDistanceMin) / distanceWindow;
mDesiredSpeed = speedOfTarget + (speedWindow * rangeScale);
writeln_d(PLATFORM.Name(), " pursue-target, speed scaled down in matching window!");
}
else if (effRange <= mMatchSpeedDistanceMin)
{
mDesiredSpeed = speedOfTarget * 0.99;
writeln_d(PLATFORM.Name(), " pursue-target, speed set to match target!");
}
if (mDesiredSpeed < mWaitSpeed)
{
mDesiredSpeed = mWaitSpeed;
writeln_d(PLATFORM.Name(), " pursue-target, speed was lower than wait speed, adjust!");
}
}
else if (mTargetTrack.LandDomain())
{
writeln_d(PLATFORM.Name(), " pursue-target, target is land domain, adjust speed!");
double speedOfTarget = mTargetTrack.Speed();
double range = PLATFORM.GroundRangeTo(mTargetTrack);
double distanceWindow = mMatchSpeedDistanceMax - mMatchSpeedDistanceMin;
double speedWindow = mInterceptSpeed - speedOfTarget;
if(range < mMatchSpeedDistanceMax && range > mMatchSpeedDistanceMin)
{
double rangeScale = (range - mMatchSpeedDistanceMin) / distanceWindow;
mDesiredSpeed = speedOfTarget + (speedWindow * rangeScale);
}
else if (range <= mMatchSpeedDistanceMin)
{
mDesiredSpeed = speedOfTarget * 0.99;
}
if (mDesiredSpeed < mWaitSpeed)
{
mDesiredSpeed = mWaitSpeed;
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,112 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior radar-control
script_debug_writes off
script_variables
bool mTurnOnFiringRadarInRange = false;
string mFiringRadarName = "radar"; // replace with sensor name on platform
double mFiringRadarRange = 1852 * 50; // 50 nautical miles (units: meters)
bool mTurnOnFiringRadarIfRWR = true;
end_script_variables
precondition
writeln_d("precondition radar-control");
if (!PROCESSOR.IsA_TypeOf("WSF_RIPR_PROCESSOR"))
{
return Failure("behavior not attached to a RIPR processor!");
}
WsfTrack curTarget = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
if ((mTurnOnFiringRadarInRange && !curTarget.IsNull() && curTarget.IsValid()) || mTurnOnFiringRadarIfRWR)
{
return true;
}
return false;
end_precondition
script bool RWR_ShowsBeingTracked()
for (int i=0; i < PLATFORM.SensorCount(); i=i+1)
{
WsfSensor sensor = PLATFORM.SensorEntry(i);
if (sensor.IsA_TypeOf("WSF_ESM_SENSOR"))
{
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
if (t.ContributorOf(PLATFORM, sensor))
{
return true;
}
}
}
}
return false;
end_script
execute
writeln_d(PLATFORM.Name(), " executing radar-control, T=", TIME_NOW);
WsfTrack curTarget = ((WsfRIPRProcessor)PROCESSOR).GetTarget();
bool turnOnFiringRadar = false;
//turn on firing radar if in range
if (mTurnOnFiringRadarInRange && !curTarget.IsNull() && curTarget.IsValid())
{
//check range against radar range
if (PLATFORM.SlantRangeTo(curTarget) < mFiringRadarRange)
{
#writeln_d(PLATFORM.Name()," within range of curTarget!");
turnOnFiringRadar = true;
}
else
{
#writeln_d(PLATFORM.Name()," NOT within range of curTarget!");
}
}
//turn on firing radar if being tracked
if (mTurnOnFiringRadarIfRWR && !turnOnFiringRadar)
{
turnOnFiringRadar = RWR_ShowsBeingTracked();
}
if (turnOnFiringRadar)
{
//make sure radar is on
WsfSensor firingRadar = PLATFORM.Sensor(mFiringRadarName);
if (firingRadar.IsValid())
{
if (!firingRadar.IsTurnedOn())
{
firingRadar.TurnOn();
writeln_d(PLATFORM.Name(), " turned on active radar ", mFiringRadarName);
}
else
{
writeln_d("radar already turned on");
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,213 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior radar-control
script_debug_writes off
script_variables
bool mTurnOnFiringRadarInRange = false;
string mFiringRadarName = "radar"; // replace with sensor name on platform
double mFiringRadarRange = 1852 * 50; // 50 nautical miles (units: meters)
bool mTurnOnFiringRadarIfRWR = true;
double cWEIGHT_SLANT_RANGE_TO = -3.0;
double cBASE_SLANT_RANGE_CONSTANT = 600000.0; //over 300 nm away
double cWEIGHT_SEPERATION_ANGLE = 20.0;
double cWEIGHT_RADARS_ON = 50.0;
double cMIN_JOB_BID = -MATH.DOUBLE_MAX();
int mRadarJobId = -1;
end_script_variables
script bool AmEmittingAnything()
for (int i=0; i<PLATFORM.SensorCount(); i=i+1)
{
WsfSensor s = PLATFORM.SensorEntry(i);
if (s.IsTurnedOn() && s.IsA_TypeOf("WSF_RADAR_SENSOR"))
{
return true;
}
}
end_script
query_bid_type radar-control
if (!JOB.IsValid())
{
return cMIN_JOB_BID;
}
Array<string> targetNames = (Array<string>)JOB.GetData("RadarTargetsArray");
WsfGeoPoint point = (WsfGeoPoint)JOB.GetData("RadarPointOfInterest");
string mode = (string)JOB.GetData("RadarMode");
string strategy = (string)JOB.GetData("RadarStrategy");
bool emitting = AmEmittingAnything();
if (mode=="silent" && emitting==true)
{
return cMIN_JOB_BID;
}
double weightSlantRange = cWEIGHT_SLANT_RANGE_TO;
if (strategy=="distributed" && mode=="support")
{
weightSlantRange = MATH.Fabs(weightSlantRange);
}
double current_bid = 10000.0;
#extern int GetBucket(double, double);
#extern double cBUCKET_BASE;
current_bid = current_bid + MATH.Fabs(cWEIGHT_SLANT_RANGE_TO) * GetBucket(cBASE_SLANT_RANGE_CONSTANT, cBUCKET_BASE);
current_bid = current_bid + weightSlantRange * GetBucket(PLATFORM.SlantRangeTo(point), cBUCKET_BASE);
WsfRIPRJob otherJob;
Set<int> deps = JOB.DependenciesForJob();
if (deps.Size()>0)
{
int dep = (int)deps.GetIterator().Data(); //just grab first one
WsfRIPRJob otherJob = PROCESSOR.GetJobById(dep);
}
if (mode=="support" && emitting==true)
{
current_bid = current_bid + cWEIGHT_RADARS_ON;
}
if (strategy=="passive" && otherJob.IsValid() && otherJob.Name()=="radar-control")
{
Array<WsfPlatform> others = otherJob.Winners();
double angle1 = point.TrueBearingTo(PLATFORM.Location());
double maxAngle = 0;
foreach(WsfPlatform other in others)
{
double seperation = MATH.NormalizeAngleMinus180_180(angle1 - point.TrueBearingTo(other.Location()));
maxAngle = MATH.Max(maxAngle, MATH.Fabs(seperation));
}
current_bid = current_bid + cWEIGHT_SEPERATION_ANGLE * GetBucket(maxAngle, cBUCKET_BASE);
}
return current_bid;
end_query_bid_type
precondition
writeln_d("precondition radar-control");
WsfRIPRJob currentJob = null;
WsfRIPRProcessor commander = PROCESSOR.GetRIPRCommanderProcessor();
if (commander.IsValid())
{
Array<int> channels = PROCESSOR.JobTypeChannels("radar-control");
if (channels.Size()>0)
{
foreach(int c in channels)
{
currentJob = commander.GetJobFor(TIME_NOW, PROCESSOR, c);
if (!currentJob.IsNull() && currentJob.IsValid() && currentJob.Name() == "radar-control")
{
mRadarJobId = currentJob.Id();
return true;
}
}
}
else
{
currentJob = PROCESSOR.GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, PROCESSOR);
if (!currentJob.IsNull() && currentJob.IsValid() && currentJob.Name() == "radar-control")
{
mRadarJobId = currentJob.Id();
return true;
}
}
}
mRadarJobId = -1;
WsfTrack curTarget = PROCESSOR.GetTarget();
if ((mTurnOnFiringRadarInRange && !curTarget.IsNull() && curTarget.IsValid()) || mTurnOnFiringRadarIfRWR)
{
return true;
}
return Failure("no radar-control job and no radar plan set!");
end_precondition
script bool RWR_ShowsBeingTracked()
for (int i=0; i < PLATFORM.SensorCount(); i=i+1)
{
WsfSensor sensor = PLATFORM.SensorEntry(i);
if (sensor.IsA_TypeOf("WSF_ESM_SENSOR"))
{
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
if (t.ContributorOf(PLATFORM, sensor))
{
return true;
}
}
}
}
return false;
end_script
execute
writeln_d("executing radar-control.");
WsfTrack curTarget = PROCESSOR.GetTarget();
bool turnOnFiringRadar = false;
//turn on firing radar if in range
if (mTurnOnFiringRadarInRange && !curTarget.IsNull() && curTarget.IsValid())
{
//check range against radar range
if (PLATFORM.SlantRangeTo(curTarget) < mFiringRadarRange)
{
#writeln_d(PLATFORM.Name()," within range of curTarget!");
turnOnFiringRadar = true;
}
else
{
#writeln_d(PLATFORM.Name()," NOT within range of curTarget!");
}
}
//turn on firing radar if being tracked
if (mTurnOnFiringRadarIfRWR && !turnOnFiringRadar)
{
turnOnFiringRadar = RWR_ShowsBeingTracked();
}
if (turnOnFiringRadar)
{
//make sure radar is on
WsfSensor firingRadar = PLATFORM.Sensor(mFiringRadarName);
if (firingRadar.IsValid())
{
if (!firingRadar.IsTurnedOn())
{
firingRadar.TurnOn();
writeln_d(PLATFORM.Name(), " turned on active radar ", mFiringRadarName);
}
else
{
writeln_d("radar already turned on");
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,85 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
#assumes the aircraft's mover is either a WSF_AIR_MOVER or a WSF_6DOF_MOVER
#assumes the mover's definition includes a max climb rate and max radial acceleration
behavior roll_over_top
script_variables
double cACCEL_GRAVITY = 9.80665; // m/s^2
double cMAX_RADIAL_ACCEL = 100 * cACCEL_GRAVITY; //larger than possible, make the aircraft limit the turn
double cMAX_CLIMB_RATE = 5000 * MATH.MPS_PER_NMPH(); //larger than possible, make the aircraft limit the climb
double cINTERCEPT_SPEED = 1260; # m/s
end_script_variables
precondition
writeln("precondition roll_over_top");
WsfTrack targetTrack = PROCESSOR.GetTarget();
WsfMover mover = PLATFORM.Mover();
if (targetTrack.IsNull() || !targetTrack.IsValid() || mover.IsNull() || !mover.IsValid())
{
writeln("target or mover not valid for executing roll_over_top");
}
else if (!mover.IsA_TypeOf("WSF_AIR_MOVER" ) && !mover.IsA_TypeOf("WSF_6DOF_MOVER"))
{
writeln("mover is not valid type for executing roll_over_top: WSF_AIR_MOVER or WSF_6DOF_MOVER");
}
else
{
//distance versus turn radius
double turnRadius = mover.TurnRadius();
double range = PLATFORM.SlantRangeTo(targetTrack);
writeln("turn radius = ", turnRadius, ", my range = ", range, ", for executing roll_over_top");
#double maxVelocityAllowed = mover.CornerVelocity(targetTrack.CurrentLocation());
#writeln("max corner speed = ", maxVelocityAllowed, ", my speed = ", PLATFORM.Speed(), ", for executing roll_over_top");
#if (PLATFORM.Speed() > maxVelocityAllowed)
#{
# return true;
#}
if ((2*turnRadius) > range)
{
return true;
}
}
return false;
end_precondition
execute
writeln("executing roll_over_top");
PLATFORM.Comment("roll_over_top");
//climb to decrease velocity (to below corner speed), while preserving energy
WsfTrack targetTrack = PROCESSOR.GetTarget();
#extern bool FlyTarget( WsfPlatform, WsfGeoPoint, double);
//turn to a higher altitude (5 nm above for now)
WsfGeoPoint pt = WsfGeoPoint.Construct(targetTrack.Latitude(), targetTrack.Longitude(), PLATFORM.Altitude() + 1852*5);
FlyTarget( PLATFORM, pt, cINTERCEPT_SPEED);
#Vec3 accelVec = PLATFORM.AccelerationWCS();
#double accel = accelVec.Magnitude();
#writeln("acceleration during ROT: ", accel, " m/s^2");
end_execute
end_behavior

View File

@@ -0,0 +1,122 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior snap_shot
script_variables
double cDIVE_ALTITUDE = 304.8; # 1000 feet
double cDEFAULT_SPEED = 200.0; # m/s
double cMIN_SPEED = 0.0; # m/s
double cGRAVITY_ACCEL = 9.80665; # 1 G
double cDEFAULT_ACCEL = (cGRAVITY_ACCEL * 7.5); # m/s^2 ~7.5 Gs
double cMAX_RADIAL_ACCEL = 100 * cGRAVITY_ACCEL; //larger than possible, make the aircraft limit the turn
end_script_variables
precondition
writeln("precondition snap_shot");
//this behavior applies when a snap shot is required and won't be suicide
WsfTrack targetTrack = PROCESSOR.GetTarget();
WsfMover mover = PLATFORM.Mover();
if (targetTrack.IsNull() || !targetTrack.IsValid() || mover.IsNull() || !mover.IsValid())
{
writeln("target or mover not valid for executing snap_shot");
return false;
}
else
{
//distance versus turn radius
#double turnRadius = mover.TurnRadius();
#double range = PLATFORM.SlantRangeTo(targetTrack);
#writeln("turn radius = ", turnRadius, ", my range = ", range);
#double maxVelocityAllowed = mover.CornerVelocity(targetTrack.CurrentLocation());
#double mySpeed = PLATFORM.Speed();
#writeln("max corner speed = ", maxVelocityAllowed, ", my speed = ", mySpeed);
//check if I've already fired at the target
if (PROCESSOR.WeaponsActive(targetTrack) > 0)
{
return false;
}
//check the relative angles and speeds between agent and the target
double relMe = MATH.Fabs( PLATFORM.RelativeBearingTo(targetTrack) );
double relHim = MATH.Fabs( targetTrack.RelativeBearingTo(PLATFORM) );
//lets try multiplying relative angles by speed
//lower relative angle and lower speed is better for snap shotting
if ((relMe * PLATFORM.Speed()) > (relHim*targetTrack.Speed()))
{
return false;
}
//check if I'm already pointed at the target
if (relMe < 5.0) //within five degrees
{
return false;
}
//check if any threats are within visual range ~20 nm
//check for side
#extern double mVisualRange;
int totalThreatsWVR = 0;
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
if (!t.SideValid() || t.Side() != PLATFORM.Side())
{
if (PLATFORM.SlantRangeTo(t) < mVisualRange)
{
totalThreatsWVR = totalThreatsWVR + 1;
}
}
}
if (totalThreatsWVR > 1) //don't slow down if other threats are nearby
{
return false;
}
/*
//check if a snapshot would help me actually fire at my target (create a fake track that has relative bearing = 0)
WsfTrack projectedTarget = WsfTrack();
projectedTarget.SetBearing(0);
projectedTarget.SetRange(PLATFORM.SlantRangeTo(targetTrack));
projectedTarget.SetElevation(PLATFORM.RelativeElevationOf(targetTrack.CurrentLocation()));
projectedTarget.SetAirDomain();
Vec3 v = targetTrack.VelocityWCS();
projectedTarget.SetVelocityWCS(v.X(), v.Y(), v.Z());
#extern string GetWeaponForThreat(WsfPlatform, WsfTrack);
string wStr = GetWeaponForThreat(PLATFORM, projectedTarget); #checks domain & kinematic capability, & valid quantity remaining
if (wStr == "")
{
writeln("checking snap_shot, projected target could NOT be fired upon");
return false;
}
*/
return true;
}
end_precondition
execute
writeln("executing snap_shot.");
PLATFORM.Comment("snap_shot");
WsfTrack targetTrack = PROCESSOR.GetTarget();
#extern bool FlyTarget( WsfPlatform, WsfGeoPoint, double);
FlyTarget( PLATFORM, targetTrack.CurrentLocation(), cMIN_SPEED);
end_execute
end_behavior

View File

@@ -0,0 +1,80 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
behavior targets_of_opportunity
script_variables
bool tempVariable;
end_script_variables
precondition
writeln("precondition targets_of_opportunity");
if (GetNumJobChannels() > 1)
{
return true;
}
return false;
end_precondition
execute
writeln("executing targets_of_opportunity.");
#extern WsfTrack GetTrackByName(WsfPlatform, string);
#extern double GetWeaponQuantityRemainingMax(WsfPlatform, Array<Map<string, Object>>);
#extern bool CheckAndFire(WsfTrack);
########################################################################
### shoot on any targets of opportunity
### we use the extra channels to represent "secondary" jobs like this
########################################################################
if (PROCESSOR.GetRIPRCommanderProcessor().IsValid())
{
for (int channel = 1; channel < GetNumJobChannels(); channel = channel + 1)
{
WsfRIPRJob aJob = GetRIPRCommanderProcessor().GetJobFor(TIME_NOW, PROCESSOR, channel);
if (!aJob.IsValid())
{
continue;
}
if (aJob.Name() == "pursue-target")
{
string targetName = (string)aJob.GetData("targetTrackName");
WsfTrack targetTrack = GetTrackByName(PLATFORM, targetName);
if (!targetTrack.IsValid() || !targetTrack.BelievedAlive()) // || !targetTrack.IFF_Foe())
{
continue;
}
//don't fire last weapon at target thats not primary target
if (GetWeaponQuantityRemainingMax(PLATFORM, mWeaponArray) > 1)
{
writeln_d("Platform: ", PLATFORM.Name(), " Looking to fire at target of opportunity: ", targetName, " for channel: ", channel);
CheckAndFire(targetTrack);
}
}
else
{
//writeln_d("Job is NOT pursue-target for channel: ", channel);
}
}
}
end_execute
end_behavior

View File

@@ -0,0 +1,122 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
#this should be used to evade a threatening enemy when you have an energy advantage
behavior unload
script_variables
double cDIVE_ALTITUDE = 304.8; # 1000 feet
double cDEFAULT_SPEED = 200.0; # m/s
double cMAX_SPEED = 9999999999.99; # m/s
double cGRAVITY_ACCEL = 9.80665; # 1 G
double cDEFAULT_ACCEL = (cGRAVITY_ACCEL * 7.5); # m/s^2 ~7.5 Gs
double cANGLE_TOLERANCE = 45.0; # degrees
double mLastProcTime = -1.0;
double mLastClimbRate = 0.0;
end_script_variables
script bool RanLastTick()
if ((TIME_NOW-mLastProcTime) < 1.5*PROCESSOR.UpdateInterval())
{
return true;
}
else return false;
end_script
precondition
writeln("precondition unload");
//see if I have the energy advantage against my threat
#extern WsfTrack mBiggestThreat;
if (mBiggestThreat.IsValid())
{
double modifier = 1.0;
if (RanLastTick())
{
modifier = 1.10;
}
//otherwise, IF we have an energy advantage, get out -> unload
###double altDiff = PLATFORM.RelativeAltitudeOf(mBiggestThreat); #positive value means track is higher than you
double altDiff = mBiggestThreat.Altitude() - PLATFORM.Altitude()*modifier; #positive value means track is higher than you
double velDiff = mBiggestThreat.Speed() - PLATFORM.Speed()*modifier; #positive value means track is faster than you
double altEnergy = MATH.Sqrt(2*MATH.Fabs(altDiff)*cGRAVITY_ACCEL); #convert potential energy into kinetic energy (mass excluded -> just convert to velocity)
if (altDiff<0)
{
altEnergy = -altEnergy;
}
if ((velDiff + altEnergy) < 0) # if I have an energy advantage, unload
{
mLastProcTime = TIME_NOW;
return true;
}
else
{
writeln(" do not unload, no energy advantage!");
}
}
mLastProcTime = -1.0;
return false;
end_precondition
execute
writeln("executing unload");
//calculate an average heading to incoming platforms weighted by distance
double y = 0;
double x = 0;
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
if (t.IsValid() && (!t.SideValid() || t.Side() != PLATFORM.Side()))
{
double distMod = 1 / PLATFORM.SlantRangeTo(t);
double bearingMod = MATH.NormalizeAngle0_360(PLATFORM.TrueBearingTo(t));
x = x + MATH.Sin(bearingMod) * distMod;
y = y + MATH.Cos(bearingMod) * distMod;
writeln_d(" Incoming ", t.TargetName(), " at distance: ", 1 / distMod, ", bearing: ", bearingMod);
}
}
if (x!=0 || y!=0) //if there is something to run from
{
double evadeHeading = MATH.NormalizeAngle0_360(MATH.ATan2(x, y) - 180);
writeln_d(" x: ", x, ", y: ", y, ", evade at: ", evadeHeading);
####extern bool FlyHold( WsfPlatform, double, double, double);
### FlyHold( PLATFORM, evadeHeading, cDIVE_ALTITUDE, cMAX_SPEED);
if (!RanLastTick())
{
mLastClimbRate = 0.0;
}
double climbRate = mLastClimbRate - (cGRAVITY_ACCEL * PROCESSOR.UpdateInterval());
mLastClimbRate = climbRate;
PLATFORM.GoToSpeed(cMAX_SPEED);
PLATFORM.TurnToHeading(evadeHeading);
PLATFORM.GoToAltitude(cDIVE_ALTITUDE, climbRate);
#PLATFORM.Comment("unload");
PLATFORM.Comment(write_str("unload to heading: ", evadeHeading));
}
end_execute
end_behavior

View File

@@ -0,0 +1,77 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
include_once behavior_disengage.txt
include_once behavior_in_danger.txt
#include_once behavior_unload.txt
#include_once behavior_force_overshoot.txt
include_once behavior_low_speed_recovery.txt
include_once behavior_avoid_overshoot.txt
include_once behavior_snap_shot.txt
include_once behavior_roll_over_top.txt
behavior within_visual_range
script_debug_writes off
script_variables
double mVisualRange = 37040.0; //meters = 20 nm
// used by behavior_in_danger, behavior_snap_shot & behavior_disengage
end_script_variables
precondition
writeln_d("precondition within_visual_range");
//check if any threats are within visual range ~20 nm
foreach (WsfLocalTrack t in PLATFORM.MasterTrackList())
{
writeln_d(" checking WVR on ", t.TargetName(), ", slant range: ", PLATFORM.SlantRangeTo(t));
if (PLATFORM.SlantRangeTo(t) < mVisualRange)
{
writeln_d(" within visual range -> apply WVR tactics");
return true;
}
}
writeln_d(" outside of visual range");
return Failure("outside of visual range");
end_precondition
execute
writeln("executing within_visual_range.");
PLATFORM.Comment("within_visual_range");
end_execute
//any child nodes get run after "execute" if the precondition is satisfied
selector #for within visual range
behavior disengage end_behavior #executes if the current force ratio compares poorly against the agent's aggressiveness
behavior in_danger end_behavior #executes if a threat has positional advantage, *** includes unload & force_overshoot ***
behavior low_speed_recovery end_behavior #executes if the platform is traveling below its desired min speed
behavior avoid_overshoot end_behavior #executes if the platform is inside the targets turning circle
behavior snap_shot end_behavior #executes if no other threats around & if speed & angle are advantageous
behavior roll_over_top end_behavior #executes if the agent needs to climb & bleed off speed to make a tighter turn towards it's target
end_selector
end_behavior

View File

@@ -0,0 +1,12 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
pushd RIPR-AIAI
source RIPR-AIAI_source.soar
popd

View File

@@ -0,0 +1,27 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
VERSION 4
\RIPR-AIAI\RIPR-AIAI.dm
0 ROOT RIPR-AIAI RIPR-AIAI 1
1 0 FOPERATOR _firstload _firstload.soar 2
2 0 FOLDER all all 3
3 2 FOPERATOR output output.soar 4
4 2 FOPERATOR wait wait.soar 5
5 0 FOLDER elaborations elaborations 6
6 5 FOPERATOR _all _all.soar 7
7 5 FOPERATOR monitor monitor.soar 8
8 5 FOPERATOR top-state top-state.soar 9
9 0 OPERATOR f-pole f-pole.soar 10
10 0 OPERATOR initialize-RIPR-AIAI initialize-RIPR-AIAI.soar 11
11 0 OPERATOR pursue-target pursue-target.soar 12
12 0 OPERATOR pursuit-lag pursuit-lag.soar 13
13 0 OPERATOR pursuit-lead pursuit-lead.soar 14
14 0 OPERATOR pursuit-pure pursuit-pure.soar 15
END

View File

@@ -0,0 +1,178 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
81
SOAR_ID 0
SOAR_ID 1
SOAR_ID 2
SOAR_ID 3
ENUMERATION 4 2 operator state
ENUMERATION 5 1 none
ENUMERATION 6 4 conflict constraint-failure no-change tie
SOAR_ID 7
FLOAT_RANGE 8 -Infinity Infinity
FLOAT_RANGE 9 -Infinity Infinity
FLOAT_RANGE 10 -Infinity Infinity
FLOAT_RANGE 11 -Infinity Infinity
FLOAT_RANGE 12 -Infinity Infinity
FLOAT_RANGE 13 -Infinity Infinity
SOAR_ID 14
FLOAT_RANGE 15 -Infinity Infinity
SOAR_ID 16
ENUMERATION 17 1 initialize-RIPR-AIAI
ENUMERATION 18 1 elaboration
SOAR_ID 19
STRING 20
SOAR_ID 21
INTEGER_RANGE 22 -2147483648 2147483647
STRING 23
SOAR_ID 24
SOAR_ID 25
ENUMERATION 26 1 pursue-target
SOAR_ID 27
STRING 28
STRING 29
STRING 30
SOAR_ID 31
ENUMERATION 32 1 wait
SOAR_ID 33
INTEGER_RANGE 34 -2147483648 2147483647
SOAR_ID 35
STRING 36
STRING 37
FLOAT_RANGE 38 -Infinity Infinity
FLOAT_RANGE 39 -Infinity Infinity
FLOAT_RANGE 40 -Infinity Infinity
FLOAT_RANGE 41 -Infinity Infinity
FLOAT_RANGE 42 -Infinity Infinity
STRING 43
SOAR_ID 44
STRING 45
ENUMERATION 46 1 nil
FLOAT_RANGE 47 -Infinity Infinity
FLOAT_RANGE 48 -Infinity Infinity
FLOAT_RANGE 49 -Infinity Infinity
SOAR_ID 50
STRING 51
FLOAT_RANGE 52 -Infinity Infinity
FLOAT_RANGE 53 -Infinity Infinity
FLOAT_RANGE 54 -Infinity Infinity
FLOAT_RANGE 55 -Infinity Infinity
FLOAT_RANGE 56 -Infinity Infinity
SOAR_ID 57
SOAR_ID 58
STRING 59
INTEGER_RANGE 60 -2147483648 2147483647
STRING 61
INTEGER_RANGE 62 -2147483648 2147483647
FLOAT_RANGE 63 -Infinity Infinity
FLOAT_RANGE 64 -Infinity Infinity
FLOAT_RANGE 65 -Infinity Infinity
INTEGER_RANGE 66 -2147483648 2147483647
INTEGER_RANGE 67 -2147483648 2147483647
FLOAT_RANGE 68 -Infinity Infinity
FLOAT_RANGE 69 -Infinity Infinity
SOAR_ID 70
SOAR_ID 71
ENUMERATION 72 2 attack recon
STRING 73
FLOAT_RANGE 74 -Infinity Infinity
FLOAT_RANGE 75 -Infinity Infinity
SOAR_ID 76
FLOAT_RANGE 77 -Infinity Infinity
STRING 78
STRING 79
STRING 80
86
0 attribute 4
0 choices 5
0 impasse 6
0 io 1
0 item 44
0 name 36
0 operator 16
0 operator 25
0 operator 31
0 stale-time 47
0 superstate 46
0 top-state 7
1 input-link 2
1 output-link 3
2 current-time 15
2 orders-root 57
2 ownship-ammo 9
2 ownship-closing-speed-max 48
2 ownship-closing-speed-min 49
2 ownship-corner-speed 12
2 ownship-engagement-range-max 11
2 ownship-engagement-range-min 10
2 ownship-speed 8
2 ownship-speed-max 13
2 ownship-threat-tolerance 63
2 ownship-weapons-incoming 67
2 point-root 70
2 track-root 14
2 weight-closing-speed-of 54
2 weight-current-target 56
2 weight-others-targeting 65
2 weight-patrol-attack 74
2 weight-patrol-recon 75
2 weight-slant-range-to 53
2 weight-weapons-in-flight 69
3 do-nothing 21
3 pursue-point 76
3 pursue-target 24
14 target 19
16 name 17
16 random 18
19 closing-speed-of 40
19 currently-targeted 55
19 others-targeting 66
19 positioning 43
19 relative-bearing-to 38
19 slant-range-to 42
19 speed 41
19 target-name 20
19 target-type 61
19 threat-level 64
19 update-time 39
19 weapons-in-flight 62
21 nothing 22
21 status 23
24 job-bid 68
24 pursuit-mode 29
24 status 30
24 target-name 28
25 actions 35
25 counted 50
25 name 26
25 target 19
27 do-nothing 33
31 actions 27
31 name 32
33 nothing 34
33 status 37
35 pursue-target 24
44 actions 35
44 name 45
44 target 19
50 name 51
50 value 52
57 prefer-target-type 58
58 target-type 59
58 weight 60
70 point 71
71 point-name 73
71 point-patrol-type 72
71 relative-bearing-to 38
71 slant-range-to 42
76 job-bid 77
76 point-name 80
76 pursuit-mode 78
76 status 79

View File

@@ -0,0 +1,21 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
source _firstload.soar
pushd all
source all_source.soar
popd
pushd elaborations
source elaborations_source.soar
popd
source f-pole.soar
source initialize-RIPR-AIAI.soar
source pursue-target.soar
source pursuit-lag.soar
source pursuit-lead.soar
source pursuit-pure.soar

View File

@@ -0,0 +1,9 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams (michael.x.williams@boeing.com)

View File

@@ -0,0 +1,10 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
source output.soar
source wait.soar

View File

@@ -0,0 +1,32 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams (michael.x.williams@boeing.com)
sp {apply*operator*create-action-command
(state <s> ^name RIPR-AIAI
^operator <o>
^io.output-link <ol>)
(<o> ^actions <act>)
(<act> ^<command-name> <command>)
(<o> ^name <oname>)
-->
(<ol> ^<command-name> <command>)
(write (crlf) |Outputting | <oname> | | <command-name>)
}
sp {apply*operator*remove-command
(state <s> ^operator.actions
^io.output-link <ol>)
(<ol> ^<att> <value>)
(<value> ^status complete)
-->
(<ol> ^<att> <value> -)
(write (crlf) |Completed | <value>)
}

View File

@@ -0,0 +1,94 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams (michael.x.williams@boeing.com)
### Propose wait for a state no-change
sp {top-ps*propose*wait
(state <s> ^name RIPR-AIAI
^io.input-link.track-root <tr>
^io.input-link.point-root <pr>
^io.input-link.current-time <time>)
(<tr> -^<target> <t>)
(<pr> -^<point> <p>)
-->
(<s> ^operator <o> + >)
(<o> ^name wait)
(<o> ^actions <a>)
(<a> ^do-nothing <dn>)
(<dn> ^nothing 0)
(write (crlf) |Propose wait++++++++++++++++++++++++++++++++++++++++++++++++|)
}
sp {top-ps*check1
(state <s> ^name RIPR-AIAI)
-->
(write (crlf) |@@@ state <s> ^name RIPR-AIAI|)
}
sp {top-ps*check2
(state <s> ^io.input-link.track-root <tr>)
-->
(write (crlf) |@@@ state <s> ^io.input-link.track-root <tr>|)
}
#sp {top-ps*check3
# (state <s> ^io.input-link.track-root <tr>)
# (<tr> -^<target> <t>)
#-->
# (write (crlf) |@@@ <tr> -^<target> <t>|)
#}
#sp {top-ps*check4
# (state <s> ^io.input-link.point-root <pr>)
#-->
# (write (crlf) |@@@ state <s> ^io.input-link.point-root <pr>|)
#}
#sp {top-ps*check5
# (state <s> ^io.input-link.point-root <pr>)
# (<pr> -^<point> <p>)
#-->
# (write (crlf) |@@@ <pr> -^<point> <p>|)
#}
#
#sp {top-ps*propose*wait
# "Propose wait if there is a state no-change."
# :default
# (state <s> ^attribute state
# ^choices none
# -^operator.name wait)
#-->
# (<s> ^operator <o> + <)
# (<o> ^name wait)
# (<o> ^actions <a>)
# (<a> ^do-nothing <dn>)
# (<dn> ^nothing 0)
# (write (crlf) |Propose wait|)
#}
### This avoids a operator no-change after wait is selected
### I've included it just to keep the trace simple - it never fires
#sp {top-ps*apply*wait*random
# "Fake production just to avoid extra operator no-change."
# :default
# (state <s> ^operator <o>)
# (<o> ^name wait)
#-->
# (<o> ^random elaboration)
#}

View File

@@ -0,0 +1,55 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
#sp {impasse-conflict*resolve*reject-non-current
# (state <s> ^superstate nil)
# (state <ss> ^superstate <s>
# ^impasse conflict
# ^item <i1>
# ^item { <i2> <> <i1> } )
# (<i1> ^name pursue-target
# ^target.currently-targeted 1
# ^target.target-name <name1>
# ^target.update-time <ut> >= <st>)
# (<i2> ^name pursue-target
# ^target.currently-targeted 0
# ^target.target-name <name2>)
#-->
# (write (crlf) |prefer-current: Operator conflict between | <name1> | & | <name2>)
# (<s> ^operator <i2> -)
#}
#sp {fix*operator*tie*impasse
# (state <s> ^attribute operator
# ^impasse tie
# ^item <i1> ^item {<i2> <> <i1> })
# (<i1> ^name pursue-target
# ^job-bid <v1>)
# (<i2> ^name pursue-target
# ^job-bid <v2>)
#-->
# (<s> ^operator <i1> =)
# (<s> ^operator <i2> =)
#}
#sp {impasse-conflict*resolve*set-indifferent
# (state <s> ^superstate nil)
# (state <ss> ^superstate <s>
# ^impasse conflict
# ^item <i1>
# ^item <i2> <> <i1>)
# (<i1> ^name <oname1>
# ^target.target-name <name1>
# (<i2> ^name <oname2>
# ^target.target-name <name2>
# ^actions.pursue-target.pursuit-mode <pm2>)
#-->
# (write (crlf) |set-indifferent: Operator conflict between | <oname1> |:| <name1> |:| <pm1>| & | <oname2> |:| <name2>|:| <pm2>)
# (<s> ^operator <i1> =)
# (<s> ^operator <i2> =)
#}

View File

@@ -0,0 +1,9 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
source impasse-conflict.soar

View File

@@ -0,0 +1,95 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0

View File

@@ -0,0 +1,43 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams (michael.x.williams@boeing.com)
#copy the io to all substates
sp {elaborate*state*io
(state <s> ^superstate.io <io>)
-->
(<s> ^io <io>)
}
#sp {elaborate*write-proposals
# (state <s> ^operator <o> +
# ^io.output-link <ol>
# ^name <sname>)
# (<o> ^actions <act>
# ^name <oname>)
# (<act> ^<command-name> <command>)
# (<command> ^<parameter-name> <parameter>)
#-->
# (write (crlf) |State: | <sname> | Proposed: | <oname> | Parameter: | <parameter> | Op: | <o>)
#}
#sp {elaborate*stale-time
# (state <s> ^io.input-link.current-time <ct>)
#-->
# (<s> ^stale-time (- <ct> 10))
#}
##copy the top state to all substates
#sp {elaborate*state*top-state
# (state <s> ^superstate.top-state <ts>)
#-->
# (<s> ^top-state <ts>)
#}

View File

@@ -0,0 +1,11 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
source _all.soar
source monitor.soar
source top-state.soar

View File

@@ -0,0 +1,106 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams michael.x.williams@boeing.com
#LBM - for debug
#sp {all*monitor*output-link
# (state <s> ^name RIPR-AIAI
# ^io.output-link <out>)
# (<out> ^<attr> <link>)
# (<link> ^{ <> status <attr2> } <value>)
# -->
# (write (crlf) | Output Command: | <attr> |.| <attr2> | | <value>)
#}
#sp {default*top-goal*halt*operator*failure
# "Halt if no operator can be selected for the top goal."
# :default
# (state <s> ^superstate nil)
# (state <ss> ^impasse conflict ^superstate <s>)
#-->
# (write (crlf) |Operator conflict.| )
# (write (crlf) |Soar must halt.| )
# (halt)
#}
#sp {default*select*reject-and-reconsider*conflict
# "Reject an object if it leads to a conflict that can not be solved."
# :default
# (state <s2> ^impasse conflict
# ^attribute operator
# ^superstate <s1>
# ^item <obj>
# ^quiescence t)
# (state <s3> ^attribute state
# ^choices none
# ^superstate <s2>
# ^quiescence t)
# (state <s1> ^operator <obj> +)
# -->
# (<s1> ^operator <obj> -)
#}
#sp {default*top-goal*halt*operator*failure
# "Halt if no operator can be selected for the top goal."
# :default
# (state <s> ^superstate nil)
# (state <ss> ^impasse constraint-failure ^superstate <s>)
#-->
# (write (crlf) |No operator can be selected for top goal.| )
# (write (crlf) |Soar must halt.| )
# (halt)
#}
#sp {impasse-conflict*resolve*prefer-closest
# (state <s> ^impasse conflict
# ^item <i1>
# ^item { <i2> <> <i1> } )
# (<i1> ^name pursue-target
# ^target.currently-targeted 1)
# (<i2> ^name pursue-target
# ^target.currently-targeted 0)
#-->
# (write (crlf) | prefer-closest: Operator conflict between | <name1> | & | <name2>)
# (<s>
#}
#### Monitor impasses I don't expect to get
#sp {all*monitor*impasse*state-no-change
# (state <s> ^attribute state
# ^choices none)
#-->
# (write (crlf) | State no-change impasse detected.|)
# (cmd |stop-soar --self|)
#}
#sp {all*monitor*operator*tie*impasse
# (state <s> ^attribute operator
# ^impasse tie
# ^item <i1> ^item {<i2> <> <i1> })
# (<i1> ^name <name1>)
# (<i2> ^name <name2>)
#-->
# (write (crlf) | Operator tie between | <name1> | & | <name2>)
# (cmd |stop-soar --self|)}
#
#sp {all*operator*impasse*tie*break
# (state <s> ^attribute operator
# ^impasse tie
# ^item <o>
# ^superstate <ss>)
#-->
# (write (crlf) | Make tied operators indifferent.|)
# (<ss> ^operator <o> =)}
#

View File

@@ -0,0 +1,15 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams (michael.x.williams@boeing.com)
#sp {elaborate*top-state*top-state
# (state <s> ^name RIPR-AIAI)
#-->
# (<s> ^top-state <s>)
#}

View File

@@ -0,0 +1,26 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
sp {propose*pursue-target*f-pole
(state <s> ^name RIPR-AIAI
^io.input-link.track-root <tr>
# ^target-name <tn>
)
(<tr> ^<target> <t>)
(<t> ^target-name <tn>
^weapons-in-flight <wif> > 0)
-->
(<s> ^operator <o> +)
(<o> ^name pursue-target
^target <t>
^actions <a>)
(<a> ^pursue-target <command>)
(<command> ^pursuit-mode f-pole
^target-name <tn>)
}

View File

@@ -0,0 +1,27 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams (michael.x.williams@boeing.com)
sp {propose*initialize-RIPR-AIAI
(state <s> ^superstate nil
-^name)
-->
(<s> ^operator <o> +)
(<o> ^name initialize-RIPR-AIAI)
(write (crlf) |Propose init|)
}
sp {apply*initialize-RIPR-AIAI
(state <s> ^operator <op>)
(<op> ^name initialize-RIPR-AIAI)
-->
(<s> ^name RIPR-AIAI)
(write (crlf) |Done with init!|)
}

View File

@@ -0,0 +1,152 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
# AUTHOR: Michael Williams (michael.x.williams@boeing.com)
### RIPR-AIAI knows how to pick a target and pursue it
sp {evaluate*pursue-target*prefer-lag
(state <s> ^name RIPR-AIAI
# ^target-name <name1>
^operator <o1> +
^operator <o2> +)
(<o1> ^name pursue-target
^actions.pursue-target.pursuit-mode lag
^target.target-name <name1>)
(<o2> ^name pursue-target
^actions.pursue-target.pursuit-mode pure
^target.target-name <name1>)
-->
(<s> ^operator <o1> > <o2>)
}
sp {evaluate*pursue-target*prefer-lead
(state <s> ^name RIPR-AIAI
# ^target-name <name1>
^operator <o1> +
^operator <o2> +)
(<o1> ^name pursue-target
^actions.pursue-target.pursuit-mode lead
^target.target-name <name1>)
(<o2> ^name pursue-target
^actions.pursue-target.pursuit-mode << pure lag >>
^target.target-name <name1>)
-->
(<s> ^operator <o1> > <o2>)
}
sp {evaluate*pursue-target*prefer-f-pole
(state <s> ^name RIPR-AIAI
# ^target-name <name1>
^operator <o1> +
^operator <o2> +)
(<o1> ^name pursue-target
^actions.pursue-target.pursuit-mode f-pole
^target.target-name <name1>)
(<o2> ^name pursue-target
^actions.pursue-target.pursuit-mode << pure lead lag >>
^target.target-name <name1>)
-->
(<s> ^operator <o1> > <o2>)
}
#sp {monitor*pursue-target*counts
# (state <s> ^name RIPR-AIAI
# ^operator <o> +)
# (<o> ^name pursue-target
# ^target <t>)
# (<t> ^target-name <name>
# ^counted <c>)
# (<c> ^name <val-name>
# ^value <v>)
#-->
# (write (crlf) |count: | <name> | | <val-name> | | <v> )
#}
#sp {evaluate*proposed-actions-for-diff-targets*indifferent
# (state <s> ^name check-target
# ^operator <o1> +
# ^operator <o2> +)
# (<o1> ^actions.<command1>.target-name <name1>)
# (<o2> ^actions.<command2>.target-name {<name2> <> <name1>})
#-->
# (<s> ^operator <o1> = <o2>)
#}
#sp {evaluate*pursue-target*prefer-high-score
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^operator { <o2> <> <o1> } +)
# (<o1> ^name pursue-target
# ^actions.pursue-target.job-bid <score1>
# ^target.target-name <name1>)
# (<o2> ^name pursue-target
# ^actions.pursue-target.job-bid { <score2> < <score1> }
# ^target.target-name <name2> <> <name1>)
#-->
# (<s> ^operator <o1> > <o2>)
# (write (crlf) |prefer-high-score: | <name1> | (| <score1> |) > | <name2> | (| <score2> |)|)
#}
#sp {evaluate*pursue-target*prefer-fastest-closer
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^operator <o2> +)
# (<o1> ^target.closing-speed-of <cso1>)
# (<o2> ^target.closing-speed-of <cso2> < <cso11>)
#-->
# (<s> ^operator <o1> > <o2>)
#}
#
#sp {evaluate*pursue-target*prefer-closest
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^operator <o2> +)
# (<o1> ^target.slant-range-to <nls1>)
# (<o2> ^target.slant-range-to <nls2> < <nls1>)
#-->
# (<s> ^operator <o1> < <o2>)
#}
#
#sp {evaluate*pursue-target*prefer-pure
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^operator <o2> +)
# (<o1> ^actions.pursue-target.pursuit-mode pure
# ^target <t>)
# (<o2> ^actions.pursue-target.pursuit-mode << lead lag >>
# ^target <t>)
#-->
# (<s> ^operator <o1> < <o2>)
#}
#
#sp {evaluate*pursue-target*prefer-lead
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^operator <o2> +)
# (<o1> ^actions.pursue-target.pursuit-mode lead
# ^target <t>)
# (<o2> ^actions.pursue-target.pursuit-mode lag
# ^target <t>)
#-->
# (<s> ^operator <o1> < <o2>)
#}
#sp {evaluate*pursue-target*prefer-current-target
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^operator <o2> +)
# (<o1> ^target.currently-targeted 0)
# (<o2> ^target.currently-targeted 1)
#-->
# (<s> ^operator <o1> < <o2>)
#}

View File

@@ -0,0 +1,38 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
sp {propose*pursue-target*lag
(state <s> ^name RIPR-AIAI
^io.input-link <il>
^io.input-link.track-root <tr>
# ^target-name <tn>
)
(<il> ^ownship-engagement-range-max <erx>)
(<tr> ^<target> <t>)
(<t> ^target-name <tn>
^slant-range-to <srt> <= <erx>
-^positioning <pos> << head-to-head head-to-tail >>)
-->
(<s> ^operator <o> +)
(<o> ^name pursue-target
^target <t>
^actions <a>)
(<a> ^pursue-target <command>)
(<command> ^pursuit-mode lag
^target-name <tn>)
}
#sp {evaluate*pursue-target*reject-lag*closing-too-slow
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^io.input-link.ownship-closing-speed-min <csm>)
# (<o1> ^actions.pursue-target.pursuit-mode lag
# ^target.closing-speed-of <cso> < <csm>)
#-->
# (<s> ^operator <o1> -)
#}

View File

@@ -0,0 +1,61 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
sp {propose*pursue-target*lead*not-head-to-slower-than-target
(state <s> ^name RIPR-AIAI
^io.input-link <il>
^io.input-link.track-root <tr>
# ^target-name <tn>
)
(<il> ^ownship-engagement-range-min <erm>
# ^ownship-engagement-range-max <erx>
^ownship-speed <os>)
(<tr> ^<target> <t>)
(<t> ^target-name <tn>
^slant-range-to <srt> >= <erm>
-^positioning <pos> << head-to-head head-to-tail >>
^speed <spd> >= <os>)
-->
(<s> ^operator <o> +)
(<o> ^name pursue-target
^target <t>
^actions <a>)
(<a> ^pursue-target <command>)
(<command> ^pursuit-mode lead
^target-name <tn>)
}
#sp {evaluate*pursue-target*reject-lead*closing-too-slow
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^io.input-link.ownship-closing-speed-min <csm>)
# (<o1> ^actions.pursue-target.pursuit-mode lead
# ^target.closing-speed-of <cso> < <csm>)
#-->
# (<s> ^operator <o1> -)
#}
#
#sp {evaluate*pursue-target*reject-lead*closing-too-fast
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^io.input-link.ownship-closing-speed-max <csx>)
# (<o1> ^actions.pursue-target.pursuit-mode lead
# ^target.closing-speed-of <cso> > <csx>)
#-->
# (<s> ^operator <o1> -)
#}
sp {evaluate*pursue-target*reject-lead*too-close
(state <s> ^name check-target
^operator <o1> +
^io.input-link.ownship-engagement-range-max <erx>)
(<o1> ^actions.pursue-target.pursuit-mode lead
^target.slant-range-to <srt> < <erx>)
-->
(<s> ^operator <o1> -)
}

View File

@@ -0,0 +1,73 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
sp {propose*pursue-target*pure*outside-max-range
(state <s> ^name RIPR-AIAI
^io.input-link <il>
^io.input-link.track-root <tr>
# ^target-name <tn>
)
(<il> ^ownship-engagement-range-max <erx>)
(<tr> ^<target> <t>)
(<t> ^target-name <tn>
^slant-range-to <srt> > <erx>)
-->
(<s> ^operator <o> +)
(<o> ^name pursue-target
^target <t>
^actions <a>)
(<a> ^pursue-target <command>)
(<command> ^pursuit-mode pure
^target-name <tn>)
}
sp {propose*pursue-target*pure*inside-max-range-head-to-x
(state <s> ^name RIPR-AIAI
^io.input-link <il>
^io.input-link.track-root <tr>
# ^target-name <tn>
)
(<il> ^ownship-engagement-range-max <erx>)
(<tr> ^<target> <t>)
(<t> ^target-name <tn>
^slant-range-to <srt> <= <erx>
^positioning <pos> << head-to-head head-to-tail >>)
-->
(<s> ^operator <o> +)
(<o> ^name pursue-target
^target <t>
^actions <a>)
(<a> ^pursue-target <command>)
(<command> ^pursuit-mode pure
^target-name <tn>)
}
#this rule may not be necessary, consider removing?
sp {evaluate*pursue-target*reject-pure*too-close-bad-positioning
(state <s> ^name RIPR-AIAI
^operator <o1> +
^io.input-link.ownship-engagement-range-max <oerx>)
(<o1> ^actions.pursue-target.pursuit-mode pure
^target.slant-range-to <srt> < <oerx>
-^target.positioning << head-to-head head-to-tail >>)
-->
(<s> ^operator <o1> -)
}
#sp {evaluate*pursue-target*reject-pure*closing-too-fast-too-close
# (state <s> ^name RIPR-AIAI
# ^operator <o1> +
# ^io.input-link.ownship-closing-speed-max <csx>
## ^io.input-link.ownship-engagement-range-max <oerx>
# )
# (<o1> ^actions.pursue-target.pursuit-mode pure
## ^target.slant-range-to <srt> < <oerx>
# ^target.closing-speed-of <cso> > <csx>)
#-->
# (<s> ^operator <o1> -)
#}

View File

@@ -0,0 +1,242 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
// functions to fly in formation
script_variables
// the proximity in which I think I've achieved the waypoint (nm)
double cWAYPOINT_PROXIMITY = 0.2 * MATH.M_PER_NM();
// the range that I think I'm far from my waypoint and need to fly a "max" speed to catch up
double cFAR_FROM_WAYPOINT = 5.0 * MATH.M_PER_NM();
// the slowest I will fly to get back into formation
double cMIN_MACH = 0.6;
// the fastest I will fly to get back into formation
double cMAX_MACH = 1.5;
// once I'm in formation, the range that I think I'm out again
double cOUT_OF_FORMATION = 2.0 * MATH.M_PER_NM();
bool mInFormation = false;
end_script_variables
script void formation_initialize(double aWaypointProximity,
double aFarFromWaypoint,
double aMinMach,
double aMaxMach,
double aOutOfFormation)
// set the constants base on the inputs
// allows users to parameterize their own formation
cWAYPOINT_PROXIMITY = aWaypointProximity;
cFAR_FROM_WAYPOINT = aFarFromWaypoint;
cMIN_MACH = aMinMach;
cMAX_MACH = aMaxMach;
cOUT_OF_FORMATION = aOutOfFormation;
end_script
script void waypoint_behind_logic(WsfWaypoint aWaypoint,
WsfPlatform aLeadPlatform,
WsfPlatform aExternalPlatform)
// what's the minimum I want to fly to let the waypoint "catch up"?
// a fraction of my lead's mach
double machToFly = aExternalPlatform.MachNumber();
double rangeToWaypoint = PLATFORM.SlantRangeTo(aWaypoint);
double relBearing = PLATFORM.RelativeBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
double absBearing = PLATFORM.TrueBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
writeln_d(" wbl, mach: ", machToFly, ", range: ", rangeToWaypoint, ", relBearing: ", relBearing);
// if I've reached my waypoint, just fly my lead's heading
if (rangeToWaypoint <= cWAYPOINT_PROXIMITY)
{
writeln_d(" in formation!");
mInFormation = true;
}
// if I'm far from my waypoint, fly a scale of my lead's speed
else if (rangeToWaypoint > cFAR_FROM_WAYPOINT)
{
writeln_d(" far from formation!");
// fly a fraction of my lead's speed and let the waypoint catch up
// machToFly = cMIN_MACH;
machToFly = cMAX_MACH;
PLATFORM.GoToMachNumber(machToFly);
// PLATFORM.GoToLocation(aWaypoint);
PLATFORM.TurnToHeading(absBearing);
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
}
// if I haven't reached my waypoint yet
else if (rangeToWaypoint > cWAYPOINT_PROXIMITY)
{
writeln_d(" close, but not in formation!");
// some speed in the middle representative of the range
// "slow down as the waypoint approaches me"
double multiplier = MATH.Sqrt(
(rangeToWaypoint - cWAYPOINT_PROXIMITY) /
(cFAR_FROM_WAYPOINT - cWAYPOINT_PROXIMITY));
multiplier = MATH.Sqrt(multiplier);
machToFly = machToFly - ((machToFly - cMIN_MACH) * multiplier);
// fly in front of the waypoint so when it reaches me, i'm flying on the same line with it
double angle = 0.0;
if (relBearing <= 0.0)
{
angle = 90.0;
}
else
{
angle = -90.0;
}
writeln_d(" angle == ", angle);
double newAngle = angle * (relBearing + 180.0) / 360.0;
writeln_d(" newAngle == ", newAngle);
if (newAngle > 45.0)
{
newAngle = 45.0;
}
if (newAngle < -45.0)
{
newAngle = -45.0;
}
writeln_d(" newAngle == ", newAngle);
double heading = aExternalPlatform.Heading() + newAngle;
writeln_d(" new heading == ", heading);
PLATFORM.TurnToHeading(heading);
PLATFORM.GoToMachNumber(machToFly);
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
}
end_script
script void waypoint_in_front_logic(WsfWaypoint aWaypoint,
WsfPlatform aLeadPlatform,
WsfPlatform aExternalPlatform)
// what's the maximum I want to fly to catch up to the waypont?
// a scale of my lead's mach
double machToFly = aExternalPlatform.MachNumber();
double rangeToWaypoint = PLATFORM.SlantRangeTo(aWaypoint);
double absBearing = PLATFORM.TrueBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
writeln_d(" wifl, mach: ", machToFly, ", range: ", rangeToWaypoint);
// if I've reached my waypoint, just fly my lead's heading
if (rangeToWaypoint <= cWAYPOINT_PROXIMITY)
{
writeln_d(" in formation!");
mInFormation = true;
PLATFORM.TurnToHeading(aExternalPlatform.Heading());
PLATFORM.GoToMachNumber(machToFly);
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
}
// if I'm really far from my waypoint, fly a scale of my lead's speed
else if (rangeToWaypoint > cFAR_FROM_WAYPOINT)
{
writeln_d(" far from formation!");
// fly a multiple of my lead's speed and catch up to the waypoint
// using lead pursuit
machToFly = cMAX_MACH;
PLATFORM.GoToMachNumber(machToFly);
// double lat = aWaypoint.Latitude();
// double lon = aWaypoint.Longitude();
// XXX ???? mSoarAirAgent.PushWaypointToLead(aWaypoint, aExternalPlatform.TruthId());
// PLATFORM.GoToLocation(aWaypoint);
PLATFORM.TurnToHeading(absBearing);
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
}
// if i haven't reached my waypoint yet
else if (rangeToWaypoint > cWAYPOINT_PROXIMITY)
{
writeln_d(" close but not in formation!");
// some speed in the middle representative of the range
// "slow down as I approach the waypoint"
double multiplier = MATH.Sqrt(
(rangeToWaypoint - (cWAYPOINT_PROXIMITY)) /
(cFAR_FROM_WAYPOINT - (cWAYPOINT_PROXIMITY)));
multiplier = MATH.Sqrt(multiplier);
machToFly = machToFly + ((cMAX_MACH - machToFly) * multiplier);
PLATFORM.GoToMachNumber(machToFly);
// double lat = aWaypoint.Latitude();
// double lon = aWaypoint.Longitude();
// XXX ???? mSoarAirAgent.PushWaypointToLead(aWaypoint, aExternalPlatform.TruthId());
PLATFORM.TurnToHeading(absBearing);
// PLATFORM.GoToLocation(aWaypoint);
}
end_script
script void formation_logic(WsfWaypoint aWaypoint,
WsfPlatform aLeadPlatform,
WsfPlatform aExternalPlatform,
bool aTightFormation)
// is waypoint behind or in front of me?
double relBearing = PLATFORM.RelativeBearingTo(aWaypoint.Latitude(), aWaypoint.Longitude(), aWaypoint.Altitude());
double waypointRange = PLATFORM.SlantRangeTo(aWaypoint);
writeln_d(" relBearing: ", relBearing, ", waypointRange: ", waypointRange);
// if waypoint is behind me and I'm somewhat close to it, just let it catch up to me
if (MATH.Fabs(relBearing) > 90 && waypointRange < 20.0 * MATH.M_PER_NM())
{
writeln_d(" waypoint behind, relBearing:", relBearing);
waypoint_behind_logic(aWaypoint, aLeadPlatform, aExternalPlatform);
}
else // waypoint is in front of me
{
writeln_d(" waypoint in front, relBearing: ", relBearing);
// if I'm in formation, don't get out of it easily
if ((aTightFormation == false) && (mInFormation == true))
{
// check if I'm still in formation
if (waypointRange >= cOUT_OF_FORMATION)
{
mInFormation = false;
}
else
{
// fly my lead's heading
PLATFORM.TurnToHeading(aExternalPlatform.Heading());
PLATFORM.GoToMachNumber(aExternalPlatform.MachNumber());
PLATFORM.GoToAltitude(aExternalPlatform.Altitude());
return;
}
}
waypoint_in_front_logic(aWaypoint, aLeadPlatform, aExternalPlatform);
}
end_script
script void formation_update(WsfPlatform aLeadPlatform)
WsfWaypoint formationPoint = WsfWaypoint();
WsfWaypoint leadPoint = WsfWaypoint();
int followingIndex = GetFollowingFormationIndex();
WsfPlatform following = mFlightLeadAgent.GetPlatformInFormation(followingIndex);
int flightLeadIndex = mFlightLeadAgent.GetFormationIndex(mFlightLeadAgent.GetPlatformInFormation(-1));
if (followingIndex == flightLeadIndex)
{
// get the formation point for my position
if ((mFlightLeadAgent.GetFormationPosition(PLATFORM, formationPoint)) == false)
{
return;
}
}
else
{
PROCESSOR.FollowPlatform(following, 110.0, 1.5 * MATH.M_PER_NM(), formationPoint);
}
writeln_d(" following: ", followingIndex, ", flightLeadIndex: ", flightLeadIndex);
writeln_d(" me: (", PLATFORM.Latitude(), ", ", PLATFORM.Longitude(), ", ", PLATFORM.Altitude(), ")");
writeln_d(" to: (", formationPoint.Latitude(), ", ", formationPoint.Longitude(), ", ", formationPoint.Altitude(), ")");
// fly formation logic
formation_logic(formationPoint, aLeadPlatform, following, false);
end_script

View File

@@ -0,0 +1,201 @@
# ****************************************************************************
# CUI
#
# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
#
# The use, dissemination or disclosure of data in this file is subject to
# limitation or restriction. See accompanying README and LICENSE for details.
# ****************************************************************************
//compile_debug true
script_variables
// TEMPORARY eventually get this from a waypoint or a battle manager
string mManeuverName = "la";
string mFlightName = "four";
int mFlightIndex = -1;
bool mFirstPass = true;
WsfAIFLProcessor mFlightLeadAgent;
end_script_variables
include_once aiai_scripts.txt
include_once formation.script
include_once la_methods.script
on_initialize
// check in to the FlightLead agent
mFlightLeadAgent = (WsfAIFLProcessor)(PROCESSOR.GetCommander());
writeln_d(" mFlightLeadAgent.IsValid() == ", mFlightLeadAgent.IsValid());
// mFlightLeadAgent.CheckIn(PLATFORM, false);
// tweak the formation flying for fueling
formation_initialize(0.2 * MATH.M_PER_NM(), // waypoint proximity
5.0 * MATH.M_PER_NM(), // far from waypoint
0.4, // min mach
1.5, // max mach
2.0 * MATH.M_PER_NM()); // out of formation
aiai_initialize();
end_on_initialize
script void lead_wait_logic()
// get the tracks from my flight
WsfLocalTrackList flightTracks = mFlightLeadAgent.FlightTrackList();
// assume targets are in-bound to my flight and find the smallest range
// target selection should be done A LOT better
double minRange = 9999999.9;
WsfTrack closestTrack = NULL;
foreach (WsfTrack track in flightTracks)
{
if (DetermineTargetType(track) == "unknown")
{
continue;
}
double thisRange = PLATFORM.SlantRangeTo(track);
writeln_d(" considering: ", track.TargetName(), " at ", thisRange);
if (thisRange < minRange)
{
minRange = thisRange;
closestTrack = track;
}
}
// if I'm within range of to start the formation tactic, do it
if (minRange < cSTART_MANEUVER_RANGE && closestTrack.IsValid())
{
writeln_d("Maneuver started at range: ", minRange);
mFlightLeadAgent.TargetSet(closestTrack);
PLATFORM.Comment("Maneuver started...");
mFlightLeadAgent.SetTopState("FLIGHT_MANEUVER");
}
if (closestTrack.IsValid())
{
mFlightLeadAgent.TargetSet(closestTrack);
PLATFORM.Comment("Range to target: " + (string)minRange);
// turn towards the track
double heading = PLATFORM.TrueBearingTo(closestTrack);
PLATFORM.TurnToHeading(heading);
PLATFORM.GoToAltitude(closestTrack.Altitude());
PLATFORM.GoToSpeed(cINTERCEPT_SPEED);
}
else
{
// PLATFORM.GoToAltitude(cDEFAULT_ALTITUDE);
// PLATFORM.GoToSpeed(cWAIT_SPEED);
}
end_script
on_update
string flightTopState = "*NO*FLIGHT*LEAD*";
writeln_d("--- four on_update Platform: ", PLATFORM.Name(), ", Time: ", TIME_NOW);
// string myTopState = PLATFORM.GetTopState();
if (!mFlightLeadAgent.IsValid())
{
mFlightLeadAgent = (WsfAIFLProcessor)(PROCESSOR.GetCommander());
if (mFlightLeadAgent.IsValid())
{
writeln_d(" acquired mFlightLeadAgent ", mFlightLeadAgent.Name());
}
else
{
// aiai_update();
// can't do anything without a FL
return;
}
}
flightTopState = mFlightLeadAgent.GetTopState();
writeln_d(" flightTopState: ", flightTopState);
// one time transition to FLIGHT_WAIT state
// if ((myTopState == "STRAIGHT") && (mFirstPass == true))
if (mFirstPass == true)
{
mFlightLeadAgent.AssignNewLead(0);
// PLATFORM.SetTopState("FLIGHT_WAIT");
mFirstPass = false;
}
// if my flight has been disrupted, just go to ai logic
if (!mFlightLeadAgent.FlightIntact())
{
aiai_update();
return;
}
// the flight is waiting for something to act on
// - lead flies straight and level
// - flight follows the lead in formation
writeln_d(" mFlightIndex: ", mFlightIndex);
// if (myTopState == "FLIGHT_WAIT")
// extern string mFlightName;
if (mFlightName == "four")
{
writeln_d(" four...");
// if lead has commanded to do the maneuver, do it
if (flightTopState == "FLIGHT_SOLO")
{
aiai_update();
return;
}
else if (flightTopState == "FLIGHT_MANEUVER")
{
writeln_d(" FLIGHT_MANEUVER...");
// get my position in the flight if not done already
// if (mFlightIndex < 0)
// {
mFlightIndex = mFlightLeadAgent.GetFormationIndex(PLATFORM) + 1;
writeln_d(" mFlightIndex acquired == ", mFlightIndex);
// }
perform_maneuver(mFlightIndex);
}
else if (flightTopState == "FLIGHT_RELEASE")
{
if (mFlightLeadAgent.GetLeadStatus(PLATFORM) == true && !mFlightLeadAgent.TargetPlatformGet().IsValid())
{
writeln_d(" target dead, FLIGHT_WAIT");
mFlightLeadAgent.SetTopState("FLIGHT_WAIT");
// PLATFORM.FollowRoute("DEFAULT_ROUTE", "CLOSEST_POINT");
PLATFORM.FollowRoute("DEFAULT_ROUTE");
return;
}
writeln_d(" FLIGHT_RELEASE...");
aiai_update();
return;
}
else
{
writeln_d(" other (", flightTopState, ")...");
if (mFlightLeadAgent.GetLeadStatus(PLATFORM) == true)
{
writeln_d(" I am lead, so wait");
SetUpdateInterval(cFAST_UPDATE_RATE);
// PLATFORM.GoToLocation(0, 0, 0); // trying to trick mover so next FollowRoute works right
// PLATFORM.FollowRoute("DEFAULT_ROUTE", "CLOSEST_POINT");
// PLATFORM.FollowRoute("line", "CLOSEST_POINT");
lead_wait_logic();
}
else
{
writeln_d(" follow the leader");
// follow the lead
WsfPlatform leadPlatform = mFlightLeadAgent.GetPlatformInFormation(-1);
SetFollowingFormationIndex(-1);
mManeuverState = 0;
SetUpdateInterval(cFAST_UPDATE_RATE);
formation_update(leadPlatform);
}
}
}
end_on_update