//------------------------------------------------------------------------------
// xyz2AzEl() -- converts relative position vector to azimuth and elevation (degs)
//------------------------------------------------------------------------------
void Sar::xyz2AzEl(const LCreal x, const LCreal y, const LCreal z, LCreal* const az, LCreal* const el)
{
// Compute azimuth (degs)
if (az != 0) {
*az = lcAtan2(y, x) * (LCreal)Basic::Angle::R2DCC;
}
if (el != 0) {
LCreal r = lcSqrt(x * x + y * y);
*el = lcAtan2(-z, r) * (LCreal)Basic::Angle::R2DCC;
}
}
//------------------------------------------------------------------------------
// xyz2AzEl() -- converts relative position vector to azimuth and elevation (degs)
//------------------------------------------------------------------------------
void Sar::xyz2AzEl(const LCreal x, const LCreal y, const LCreal z, LCreal* const az, LCreal* const el)
{
// Compute azimuth (degs)
if (az != nullptr) {
*az = lcAtan2(y, x) * static_cast<LCreal>(Basic::Angle::R2DCC);
}
if (el != nullptr) {
const LCreal r = lcSqrt(x * x + y * y);
*el = lcAtan2(-z, r) * static_cast<LCreal>(Basic::Angle::R2DCC);
}
}
//------------------------------------------------------------------------------
// Check to see if two positions are over the horizon from each other.
// return TRUE if they are both within horizon distance, FALSE if they
// are over the horizon.
//------------------------------------------------------------------------------
bool TdbIr::horizonCheck(const osg::Vec3& position1, const osg::Vec3& position2)
{
bool aboveHorizon = true;
//LET .FIRST.NODE.DISTANCE.TO.HORIZON
// = SQRT.F(MAX.F (2.0 * EARTH.RADIUS * .FIRST.NODE.POSITION(3), 1.0) )
LCreal distance1 = lcSqrt( LCreal(2.0f * Basic::Nav::ERADM * -position1.z()) );
if (distance1 < 1.0f) distance1 = 1.0f;
// LET .SECOND.NODE.DISTANCE.TO.HORIZON
// = SQRT.F(MAX.F (2.0 * EARTH.RADIUS * .SECOND.NODE.POSITION(3), 1.0) )
LCreal distance2 = lcSqrt( LCreal(2.0f * Basic::Nav::ERADM * -position2.z()) );
if (distance2 < 1.0f) distance2 = 1.0f;
//LET .RELATIVE.POSITION(*)
// = UT.LINEAR.COMBINATION.OF.VECTORS.F(1.0, .FIRST.NODE.POSITION(*),
// -1.0, .SECOND.NODE.POSITION(*))
// LET .RELATIVE.POSITION(3) = 0.0
// LET .GROUND.TRACK.RANGE = UT.NORM.F(.RELATIVE.POSITION(*))
osg::Vec3 groundVec = position1 - position2;
LCreal gndRng = lcSqrt ((groundVec.x() * groundVec.x())
+ (groundVec.y() * groundVec.y()));
//IF .GROUND.TRACK.RANGE < .FIRST.NODE.DISTANCE.TO.HORIZON
// + .SECOND.NODE.DISTANCE.TO.HORIZON
if (gndRng >= distance1 + distance2)
aboveHorizon = false;
return aboveHorizon;
}
//------------------------------------------------------------------------------
// Process players-of-interest --- Scan the provided player list and compute range,
// range rate, normalized Line-Of-Sight (LOS) vectors for each target player.
// (Background task)
//------------------------------------------------------------------------------
unsigned int TdbIr::processPlayers(Basic::PairStream* const players)
{
// Clear the old data
clearArrays();
// ---
// Early out checks (no ownship, no players of interest, no target data arrays)
// ---
if (gimbal == 0 || ownship == 0 || players == 0 || maxTargets == 0) return 0;
//const Basic::Pair* p = ((Player*)ownship)->getIrSystemByType( typeid(IrSensor) );
//if (p == 0) return 0;
// FAB - refactored
//const IrSensor* irSensor = (const IrSensor*)( p->object() );
//if (irSensor == 0)
//return 0;
// FAB - limit is +/- (1/2 FORtheta + 1/2 IFOVtheta) (but both get..Theta() actually return 1/2 Theta)
//LCreal fieldOfRegardTheta = irSensor->getFieldOfRegardTheta() + irSensor->getIFOVTheta();
//LCreal maxRange = irSensor->getMaximumRange();
// FAB - basically the same as TDB/gimbal version:
const double maxRange = gimbal->getMaxRange2PlayersOfInterest();
const double maxAngle = gimbal->getMaxAngle2PlayersOfInterest();
// ---
// Get our position and velocity vectors
// ## using ownship position for now; should include the location of the gimbal ##
// ---
osg::Vec3 p0 = ownship->getPosition(); // Position Vector
osg::Vec3 v0 = ownship->getVelocity(); // Ownship Velocity Vector
// ---
// 1) Scan the player list --- compute the normalized Line-Of-Sight (LOS) vectors,
// range, and range rate for each target.
// ---
for (Basic::List::Item* item = players->getFirstItem(); item != 0 && numTgts < maxTargets; item = item->getNext()) {
// Get the pointer to the target player
Basic::Pair* pair = (Basic::Pair*)(item->getValue());
Player* target = (Player*)(pair->object());
// FAB - testing - exclude our launch vehicle in tdb
//if (ownship->isMajorType(Player::WEAPON) && ((Weapon*)ownship)->getLaunchVehicle() == target)
//continue;
if (target != ownship && target->isActive()) {
bool aboveHorizon = true;
aboveHorizon = horizonCheck (ownship->getPosition(), target->getPosition());
// FAB - refactored - don't continue if we know we're excluding this target
if (!aboveHorizon) continue;
// Determine if target is within azimuth and elevation checks. If it is, keep it.
// Otherwise, reject.
osg::Vec3 targetPosition = target->getPosition();
osg::Vec3 losVector = targetPosition - p0;
//osg::Vec3 xlos = -losVector;
LCreal aazr;
LCreal aelr;
LCreal ra;
if (irSensor->getSeeker()->getOwnHeadingOnly()) {
// FAB - this calc for gimbal ownHeadingOnly true
// compute ranges
LCreal gndRng2 = losVector.x()*losVector.x() + losVector.y()*losVector.y();
ra = lcSqrt(gndRng2);
// compute angles
LCreal los_az = lcAtan2(losVector.y(),losVector.x());
double hdng = ownship->getHeadingR();
aazr = lcAepcRad(los_az - (float)hdng);
aelr = lcAtan2(-losVector.z(), ra);
}
else {
// FAB - this calc for gimbal ownHeadingOnly false
//osg::Vec4 los0( losVector.x(), losVector.y(), losVector.z(), 0.0 );
//osg::Vec4 aoi = ownship->getRotMat() * los0;
osg::Vec3 aoi = ownship->getRotMat() * losVector;
// 3) Compute the azimuth and elevation angles of incidence (AOI)
// 3-a) Get the aoi vector values & compute range squared
LCreal xa = aoi.x();
LCreal ya = aoi.y();
LCreal za = -aoi.z();
ra = lcSqrt(xa*xa + ya*ya);
// 3-b) Compute azimuth: az = atan2(ya, xa)
aazr = lcAtan2(ya, xa);
// 3-c) Compute elevation: el = atan2(za, ra), where 'ra' is sqrt of xa*xa & ya*ya
aelr = lcAtan2(za,ra);
}
LCreal absoluteAzimuth = aazr;
if (aazr < 0) absoluteAzimuth = -aazr;
LCreal absoluteElevation = aelr;
if (aelr < 0) absoluteElevation = -aelr;
bool withinView = true;
// LCreal fieldOfRegardTheta = 0;
// LCreal sensorMaxRange = 0;
// {
// //const Basic::Pair* p = ((Player*)ownship)->getIrSystemByType( typeid(IrSensor) );
// //if (p != 0) {
// //const IrSensor* irSensor = (const IrSensor*)( p->object() );
// // fieldOfRegardTheta = irSensor->getFieldOfRegardTheta();
//// FAB - limit is +/- (1/2 FORtheta + 1/2 IFOVtheta) (but both get..Theta() actually return 1/2 Theta)
//fieldOfRegardTheta = irSensor->getFieldOfRegardTheta() + irSensor->getIFOVTheta();
// sensorMaxRange = irSensor->getMaximumRange();
// //}
// }
if ((absoluteAzimuth > maxAngle) || // outside field of view
(absoluteElevation > maxAngle) ||
(ra > maxRange) || // beyond max range of sensor
!aboveHorizon)
withinView = false;
if (withinView) {
// Ref() and save the target pointer
// (## It must be unref()'d by the owner/manager of the Tdb array ##)
target->ref();
targets[numTgts] = target;
// FAB - these seem to be unnecessary - recalc'd by Tdb::computeBoresightData anyway
// Line-Of-Sight (LOS) vector (world)
//losO2T[numTgts] = target->getPosition() - p0;
// Normalized and compute length [unit vector and range(meters)]
//ranges[numTgts] = losO2T[numTgts].normalize();
// Computer range rate (meters/sec)
//rngRates[numTgts] = (LCreal) ((target->getVelocity() - v0) * losO2T[numTgts]);
// Save the target pointer (for quick access)
numTgts++;
}
} //(target != ownship && target->isActive())
}
return numTgts;
}
//------------------------------------------------------------------------------
// weaponGuidance() -- default guidance; using Robot Aircraft (RAC) guidance
//------------------------------------------------------------------------------
void Missile::weaponGuidance(const LCreal dt)
{
// ---
// Control velocity: During burn time, accel to max velocity,
// after burn time, deaccelerate to min velocity.
// ---
if (isEngineBurnEnabled()) cmdVelocity = vpMax;
else cmdVelocity = vpMin;
// ---
// If the target's already dead,
// then don't go away mad, just go away.
// ---
const Player* tgt = getTargetPlayer();
const Track* trk = getTargetTrack();
if (trk != 0) tgt = trk->getTarget();
if (tgt != 0 && !tgt->isActive()) return;
osg::Vec3 los; // Target Line of Sight
osg::Vec3 vel; // Target velocity
// ---
// Basic guidance
// ---
{
// ---
// Get position and velocity vectors from the target/track
// ---
osg::Vec3 posx;
calculateVectors(tgt, trk, &los, &vel, &posx);
// compute range to target
LCreal trng0 = trng;
trng = los.length();
// compute range rate,
LCreal trdot0 = trdot;
if (dt > 0)
trdot = (trng - trng0)/dt;
else
trdot = 0;
// Target total velocit
LCreal totalVel = vel.length();
// compute target velocity parallel to LOS,
LCreal vtplos = (los * vel/trng);
// ---
// guidance - fly to intercept point
// ---
// if we have guidance ...
if ( isGuidanceEnabled() && trng > 0) {
// get missile velocity (must be faster than target),
LCreal v = vpMax;
if (v < totalVel) v = totalVel + 1;
// compute target velocity normal to LOS squared,
LCreal tgtVp = totalVel;
LCreal vtnlos2 = tgtVp*tgtVp - vtplos*vtplos;
// and compute missile velocity parallex to LOS.
LCreal vmplos = lcSqrt( v*v - vtnlos2 );
// Now, use both velocities parallel to LOS to compute
// closure rate.
LCreal vclos = vmplos - vtplos;
// Use closure rate and range to compute time to intercept.
LCreal dt1 = 0;
if (vclos > 0) dt1 = trng/vclos;
// Use time to intercept to extrapolate target position.
osg::Vec3 p1 = (los + (vel * dt1));
// Compute missile commanded heading and
cmdHeading = lcAtan2(p1.y(),p1.x());
// commanded pitch.
LCreal grng = lcSqrt(p1.x()*p1.x() + p1.y()*p1.y());
cmdPitch = -lcAtan2(p1.z(),grng);
}
}
// ---
// fuzing logic (let's see if we've scored a hit)
// (compute range at closest point and compare to max burst radius)
// (use target truth data)
// ---
{
// ---
// Get position and velocity vectors from the target (truth)
// (or default to the values from above)
// ---
if (tgt != 0) {
calculateVectors(tgt, 0, &los, &vel, 0);
}
// compute range to target
LCreal trng0 = trngT;
trngT = los.length();
// compute range rate,
LCreal trdot0 = trdotT;
if (dt > 0)
trdotT = (trngT - trng0)/dt;
else
trdotT = 0;
// when we've just passed the target ...
if (trdotT > 0 && trdot0 < 0 && !isDummy() && getTOF() > 2.0f) {
bool missed = true; // assume the worst
// compute relative velocity vector.
osg::Vec3 velRel = (vel - getVelocity());
// compute missile velocity squared,
LCreal vm2 = velRel.length2();
if (vm2 > 0) {
// relative range (dot) relative velocity
LCreal rdv = los * velRel;
// interpolate back to closest point
LCreal ndt = -rdv/vm2;
osg::Vec3 p0 = los + (velRel*ndt);
// range squared at closest point
LCreal r2 = p0.length2();
// compare to burst radius squared
if (r2 <= (getMaxBurstRng()*getMaxBurstRng()) ) {
// We've detonated
missed = false;
setMode(DETONATED);
setDetonationResults( DETONATE_ENTITY_IMPACT );
// compute location of the detonation relative to the target
osg::Vec3 p0n = -p0;
if (tgt != 0) p0n = tgt->getRotMat() * p0n;
setDetonationLocation(p0n);
// Did we hit anyone?
checkDetonationEffect();
// Log the event
LCreal detRange = getDetonationRange();
if (isMessageEnabled(MSG_INFO)) {
std::cout << "DETONATE_ENTITY_IMPACT rng = " << detRange << std::endl;
}
if (getAnyEventLogger() != 0) {
TabLogger::TabLogEvent* evt = new TabLogger::LogWeaponActivity(2, getLaunchVehicle(), this, getTargetPlayer(), DETONATE_ENTITY_IMPACT, detRange); // type 2 for "detonate"
getAnyEventLogger()->log(evt);
evt->unref();
}
}
}
// Did we miss the target?
if (missed) {
// We've detonated ...
setMode(DETONATED);
setDetonationResults( DETONATE_DETONATION );
// because we've just missed the target
setTargetPlayer(0,false);
setTargetTrack(0,false);
// Log the event
LCreal detRange = trngT;
if (isMessageEnabled(MSG_INFO)) {
std::cout << "DETONATE_OTHER rng = " << detRange << std::endl;
}
if (getAnyEventLogger() != 0) {
TabLogger::TabLogEvent* evt = new TabLogger::LogWeaponActivity(2, getLaunchVehicle(), this, getTargetPlayer(), DETONATE_DETONATION, getDetonationRange()); // type 2 for "detonate"
getAnyEventLogger()->log(evt);
evt->unref();
}
}
}
}
}
