//------------------------------------------------------------------------------ // 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(); } } } } }