//------------------------------------------------------------------------------
// updateData() -
//------------------------------------------------------------------------------
void Adi::updateData(const LCreal dt)
{
// update our base class first
BaseClass::updateData(dt);
// drive our adi toward the actual pitch, from our current pitch, no faster
// than our MAX_RATE (this allows for greater fidelity, simulates an analog adi)
LCreal delta = 0;
delta = alim (lcAepcDeg(pitch - curTheta), maxRate * dt);
curTheta = lcAepcDeg(curTheta + delta);
// now do the same thing for roll
delta = alim (lcAepcRad(roll - curPhi), maxRate * dt);
curPhi = lcAepcRad(curPhi + delta);
// get our table, and do the linear interpolation ourself
setInstVal(curTheta);
scaledPitch = getInstValue();
}
//------------------------------------------------------------------------------
// updateData() - updates our non time-critical threads here
//------------------------------------------------------------------------------
void BearingPointer::updateData(const LCreal dt)
{
// update our base class first
BaseClass::updateData(dt);
// get our heading and bearing (hopefully they are in radians, if not, the
// calculation will be skewed
LCreal hdg = getRotationRad();
// stay between +- 3.14 radians
bearing = lcAepcRad(bearing - hdg);
LCreal dbrg = lcAepcRad(myRotation - bearing);
// if we are over the max, rotate the other way
LCreal dd0 = dbrg * dt;
LCreal maxdd0 = (90.0f * (LCreal) Basic::Angle::D2RCC) * dt; // Limit to 90 degs/sec
if (dd0 < -maxdd0) dd0 = -maxdd0;
if (dd0 > maxdd0) dd0 = maxdd0;
bearing += dd0;
myRotation = bearing;
}
//------------------------------------------------------------------------------
// 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;
}
void LifeForm::look(const LCreal up, const LCreal sdws)
{
if (getDamage() < 1) {
if (lockMode != LOCKED) {
lockMode = SEARCHING;
// our up and sideways come in as -5 to 5, which is a rate to adjust heading
const osg::Vec3 old = getEulerAngles();
LCreal hdg = old.z();
LCreal ptc = lookAngle;
LCreal tempSdws = sdws;
LCreal tempUp = up;
if (lcAbs(tempSdws) < 0.00005) tempSdws = 0;
if (lcAbs(tempUp) < 0.05) tempUp = 0;
hdg += tempSdws;
hdg = lcAepcRad(hdg);
// we don't change our pitch when we look up and down, we only change our look angle, so we have to keep
// that separate. WE do, however, change our heading based on where we are looking, so that is correct
ptc += tempUp;
if (ptc > 90) ptc = 90;
else if (ptc < -90) ptc = -90;
//std::cout << "HEADING = " << hdg << std::endl;
setLookAngle(ptc);
osg::Vec3 eul(0, 0, hdg);
setEulerAngles(eul);
// now based on this we need to know if we have a target in our crosshairs...
tgtAquired = false;
if (tgtPlayer != nullptr) tgtPlayer->unref();
tgtPlayer = nullptr;
const osg::Vec3 myPos = getPosition();
osg::Vec3 tgtPos;
osg::Vec3 vecPos;
LCreal az = 0.0, el = 0.0, range = 0.0, diffAz = 0.0, diffEl = 0.0;
const LCreal maxAz = (0.7f * static_cast<LCreal>(Basic::Angle::D2RCC));
const LCreal maxEl = (0.7f * static_cast<LCreal>(Basic::Angle::D2RCC));
//LCreal maxRange = 1500.0f; // long range right now
const LCreal la = lookAngle * static_cast<LCreal>(Basic::Angle::D2RCC);
Simulation* sim = getSimulation();
if (sim != nullptr) {
Basic::PairStream* players = sim->getPlayers();
if (players != nullptr) {
Basic::List::Item* item = players->getFirstItem();
while (item != nullptr && !tgtAquired) {
Basic::Pair* pair = static_cast<Basic::Pair*>(item->getValue());
if (pair != nullptr) {
Player* player = dynamic_cast<Player*>(pair->object());
if (player != nullptr && player != this && !player->isMajorType(WEAPON) && !player->isDestroyed()) {
// ok, calculate our position from this guy
tgtPos = player->getPosition();
vecPos = tgtPos - myPos;
az = lcAtan2(vecPos.y(), vecPos.x());
range = (vecPos.x() * vecPos.x() + vecPos.y() * vecPos.y());
range = std::sqrt(range);
// now get our elevation
el = lcAtan2(-vecPos.z(), range);
diffAz = lcAbs(lcAepcRad(az - static_cast<LCreal>(getHeadingR())));
diffEl = lcAbs(lcAepcRad(la - el));
if ((diffAz <= maxAz) && (diffEl <= maxEl)) {
lockMode = TGT_IN_SIGHT;
tgtAquired = true;
if (tgtPlayer != player) {
if (tgtPlayer != nullptr) tgtPlayer->unref();
tgtPlayer = player;
tgtPlayer->ref();
}
}
}
}
item = item->getNext();
}
players->unref();
players = nullptr;
}
}
}
// else we are locking on target, and need to follow our target player
else {
if (tgtPlayer == nullptr) lockMode = SEARCHING;
else {
const osg::Vec3 vecPos = tgtPlayer->getPosition() - getPosition();
const LCreal az = lcAtan2(vecPos.y(), vecPos.x());
LCreal range = (vecPos.x() * vecPos.x() + vecPos.y() * vecPos.y());
range = std::sqrt(range);
// now get our elevation
const LCreal el = lcAtan2(-vecPos.z(), range);
// now force that on us
setLookAngle(el * static_cast<LCreal>(Basic::Angle::R2DCC));
setEulerAngles(0, 0, az);
}
}
}
}
//------------------------------------------------------------------------------
// weaponDynamics -- default missile dynamics; using Robot Aircraft (RAC) dynamics
//------------------------------------------------------------------------------
void Missile::weaponDynamics(const LCreal dt)
{
static const LCreal g = ETHG; // Acceleration of Gravity
// ---
// Max turning G (Missiles: Use Gmax)
// ---
LCreal gmax = maxG;
// ---
// Computer max turn rate, max/min pitch rates
// ---
// Turn rate base on vp and g,s
LCreal ra_max = gmax * g / getTotalVelocity();
// Set max (pull up) pitch rate same as turn rate
LCreal qa_max = ra_max;
// Set min (push down) pitch rate
LCreal qa_min = -qa_max;
// ---
// Get old angular values
// ---
const osg::Vec3 oldRates = getAngularVelocities();
//LCreal pa1 = oldRates[IROLL];
LCreal qa1 = oldRates[IPITCH];
LCreal ra1 = oldRates[IYAW];
// ---
// Find pitch rate and update pitch
// ---
LCreal qa = lcAepcRad(cmdPitch - (LCreal) getPitchR());
if(qa > qa_max) qa = qa_max;
if(qa < qa_min) qa = qa_min;
// Using Pitch rate, integrate pitch
LCreal newTheta = (LCreal) (getPitch() + (qa + qa1) * dt / 2.0);
// Find turn rate
LCreal ra = lcAepcRad(cmdHeading - (LCreal) getHeadingR());
if(ra > ra_max) ra = ra_max;
if(ra < -ra_max) ra = -ra_max;
// Use turn rate integrate heading
LCreal newPsi = (LCreal) (getHeading() + (ra + ra1) * dt / 2.0);
if(newPsi > 2.0f*PI) newPsi -= (LCreal)(2.0*PI);
if(newPsi < 0.0f) newPsi += (LCreal)(2.0*PI);
// Roll angle proportional to max turn rate - filtered
LCreal pa = 0.0;
LCreal newPhi = (LCreal) ( 0.98 * getRollR() + 0.02 * ((ra / ra_max) * (Basic::Angle::D2RCC * 60.0)) );
// Sent angular values
setEulerAngles(newPhi, newTheta, newPsi);
setAngularVelocities(pa, qa, ra);
// Find Acceleration
LCreal vpdot = (cmdVelocity - getTotalVelocity());
if(vpdot > maxAccel) vpdot = maxAccel;
if(vpdot < -maxAccel) vpdot = -maxAccel;
// Set acceleration vector
osg::Vec3 aa(vpdot, 0.0, 0.0);
osg::Vec3 ae = aa * getRotMat();
setAcceleration(ae);
// Comute new velocity
LCreal newVP = getTotalVelocity() + vpdot * dt;
// Set acceleration vector
osg::Vec3 ve0 = getVelocity();
osg::Vec3 va(newVP, 0.0, 0.0);
osg::Vec3 ve1 = va * getRotMat();
setVelocity(ve1);
setVelocityBody(newVP, 0.0, 0.0);
}