X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAICarrier.cxx;h=b6487519d026c05ea55bc026423f093d30ce9375;hb=b4fbde72b275dc75e8ecf7b44261aba642e5da0f;hp=d4c6ddba6285eb6d39133c21d7d1b700d838c8d5;hpb=4bfd1722df24f5be89459b25010e5d7352720a7b;p=flightgear.git diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index d4c6ddba6..b6487519d 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -15,28 +15,26 @@ // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. #ifdef HAVE_CONFIG_H # include #endif +#include #include #include -#include +#include +#include #include + #include #include
#include
#include "AICarrier.hxx" -/** Value of earth radius (meters) */ -#define RADIUS_M SG_EQUATORIAL_RADIUS_M - - - FGAICarrier::FGAICarrier() : FGAIShip(otCarrier) { } @@ -58,45 +56,32 @@ void FGAICarrier::readFromScenario(SGPropertyNode* scFileNode) { setMinLat(scFileNode->getDoubleValue("min-lat", 0)); setMaxLong(scFileNode->getDoubleValue("max-long", 0)); setMinLong(scFileNode->getDoubleValue("min-long", 0)); + setMPControl(scFileNode->getBoolValue("mp-control", false)); + setAIControl(scFileNode->getBoolValue("ai-control", false)); SGPropertyNode* flols = scFileNode->getChild("flols-pos"); if (flols) { - flols_off[0] = flols->getDoubleValue("x-offset-m", 0); - flols_off[1] = flols->getDoubleValue("y-offset-m", 0); - flols_off[2] = flols->getDoubleValue("z-offset-m", 0); + // Transform to the right coordinate frame, configuration is done in + // the usual x-back, y-right, z-up coordinates, computations + // in the simulation usual body x-forward, y-right, z-down coordinates + flols_off(0) = - flols->getDoubleValue("x-offset-m", 0); + flols_off(1) = flols->getDoubleValue("y-offset-m", 0); + flols_off(2) = - flols->getDoubleValue("z-offset-m", 0); } else - flols_off = Point3D(0, 0, 0); + flols_off = SGVec3d::zeros(); - std::vector props = scFileNode->getChildren("wire"); + std::vector props = scFileNode->getChildren("parking-pos"); std::vector::const_iterator it; - for (it = props.begin(); it != props.end(); ++it) { - std::string s = (*it)->getStringValue(); - if (!s.empty()) - wire_objects.push_back(s); - } - - props = scFileNode->getChildren("catapult"); - for (it = props.begin(); it != props.end(); ++it) { - std::string s = (*it)->getStringValue(); - if (!s.empty()) - catapult_objects.push_back(s); - } - - props = scFileNode->getChildren("solid"); - for (it = props.begin(); it != props.end(); ++it) { - std::string s = (*it)->getStringValue(); - if (!s.empty()) - solid_objects.push_back(s); - } - - props = scFileNode->getChildren("parking-pos"); for (it = props.begin(); it != props.end(); ++it) { string name = (*it)->getStringValue("name", "unnamed"); - double offset_x = (*it)->getDoubleValue("x-offset-m", 0); + // Transform to the right coordinate frame, configuration is done in + // the usual x-back, y-right, z-up coordinates, computations + // in the simulation usual body x-forward, y-right, z-down coordinates + double offset_x = -(*it)->getDoubleValue("x-offset-m", 0); double offset_y = (*it)->getDoubleValue("y-offset-m", 0); - double offset_z = (*it)->getDoubleValue("z-offset-m", 0); + double offset_z = -(*it)->getDoubleValue("z-offset-m", 0); double hd = (*it)->getDoubleValue("heading-offset-deg", 0); - ParkPosition pp(name, Point3D(offset_x, offset_y, offset_z), hd); + ParkPosition pp(name, SGVec3d(offset_x, offset_y, offset_z), hd); ppositions.push_back(pp); } } @@ -133,153 +118,100 @@ void FGAICarrier::setTACANChannelID(const string& id) { TACAN_channel_id = id; } -void FGAICarrier::getVelocityWrtEarth(sgdVec3& v, sgdVec3& omega, sgdVec3& pivot) { - sgdCopyVec3(v, vel_wrt_earth ); - sgdCopyVec3(omega, rot_wrt_earth ); - sgdCopyVec3(pivot, rot_pivot_wrt_earth ); +void FGAICarrier::setMPControl(bool c) { + MPControl = c; } -void FGAICarrier::update(double dt) { - // For computation of rotation speeds we just use finite differences her. - // That is perfectly valid since this thing is not driven by accelerations - // but by just apply discrete changes at its velocity variables. - double old_hdg = hdg; - double old_roll = roll; - double old_pitch = pitch; - - // Update the velocity information stored in those nodes. - double v_north = 0.51444444*speed*cos(hdg * SGD_DEGREES_TO_RADIANS); - double v_east = 0.51444444*speed*sin(hdg * SGD_DEGREES_TO_RADIANS); - - double sin_lat = sin(pos.lat() * SGD_DEGREES_TO_RADIANS); - double cos_lat = cos(pos.lat() * SGD_DEGREES_TO_RADIANS); - double sin_lon = sin(pos.lon() * SGD_DEGREES_TO_RADIANS); - double cos_lon = cos(pos.lon() * SGD_DEGREES_TO_RADIANS); - double sin_roll = sin(roll * SGD_DEGREES_TO_RADIANS); - double cos_roll = cos(roll * SGD_DEGREES_TO_RADIANS); - double sin_pitch = sin(pitch * SGD_DEGREES_TO_RADIANS); - double cos_pitch = cos(pitch * SGD_DEGREES_TO_RADIANS); - double sin_hdg = sin(hdg * SGD_DEGREES_TO_RADIANS); - double cos_hdg = cos(hdg * SGD_DEGREES_TO_RADIANS); - - // Transform this back the the horizontal local frame. - sgdMat3 trans; - - // set up the transform matrix - trans[0][0] = cos_pitch*cos_hdg; - trans[0][1] = sin_roll*sin_pitch*cos_hdg - cos_roll*sin_hdg; - trans[0][2] = cos_roll*sin_pitch*cos_hdg + sin_roll*sin_hdg; - - trans[1][0] = cos_pitch*sin_hdg; - trans[1][1] = sin_roll*sin_pitch*sin_hdg + cos_roll*cos_hdg; - trans[1][2] = cos_roll*sin_pitch*sin_hdg - sin_roll*cos_hdg; - - trans[2][0] = -sin_pitch; - trans[2][1] = sin_roll*cos_pitch; - trans[2][2] = cos_roll*cos_pitch; - - sgdSetVec3( vel_wrt_earth, - - cos_lon*sin_lat*v_north - sin_lon*v_east, - - sin_lon*sin_lat*v_north + cos_lon*v_east, - cos_lat*v_north ); - sgGeodToCart(pos.lat() * SGD_DEGREES_TO_RADIANS, - pos.lon() * SGD_DEGREES_TO_RADIANS, - pos.elev(), rot_pivot_wrt_earth); +void FGAICarrier::setAIControl(bool c) { + AIControl = c; +} +void FGAICarrier::update(double dt) { // Now update the position and heading. This will compute new hdg and // roll values required for the rotation speed computation. FGAIShip::update(dt); - //automatic turn into wind with a target wind of 25 kts otd - if(turn_to_launch_hdg){ - TurnToLaunch(); - } else if(OutsideBox() || returning) {// check that the carrier is inside the operating box - ReturnToBox(); + //SG_LOG(SG_GENERAL, SG_ALERT, "AICarrier: MPControl " << MPControl << " AIControl " << AIControl); + if (!MPControl && AIControl){ + + if(turn_to_launch_hdg){ + TurnToLaunch(); + } else if(turn_to_recovery_hdg ){ + TurnToRecover(); + } else if(OutsideBox() || returning ) {// check that the carrier is inside + ReturnToBox(); // the operating box, + } else { + TurnToBase(); + } + } else { - TurnToBase(); + FGAIShip::TurnTo(tgt_heading); + FGAIShip::AccelTo(tgt_speed); } - // Only change these values if we are able to compute them safely - if (dt < DBL_MIN) - sgdSetVec3( rot_wrt_earth, 0.0, 0.0, 0.0); - else { - // Compute the change of the euler angles. - double hdg_dot = SGD_DEGREES_TO_RADIANS * (hdg-old_hdg)/dt; - // Always assume that the movement was done by the shorter way. - if (hdg_dot < - SGD_DEGREES_TO_RADIANS * 180) - hdg_dot += SGD_DEGREES_TO_RADIANS * 360; - if (hdg_dot > SGD_DEGREES_TO_RADIANS * 180) - hdg_dot -= SGD_DEGREES_TO_RADIANS * 360; - double pitch_dot = SGD_DEGREES_TO_RADIANS * (pitch-old_pitch)/dt; - // Always assume that the movement was done by the shorter way. - if (pitch_dot < - SGD_DEGREES_TO_RADIANS * 180) - pitch_dot += SGD_DEGREES_TO_RADIANS * 360; - if (pitch_dot > SGD_DEGREES_TO_RADIANS * 180) - pitch_dot -= SGD_DEGREES_TO_RADIANS * 360; - double roll_dot = SGD_DEGREES_TO_RADIANS * (roll-old_roll)/dt; - // Always assume that the movement was done by the shorter way. - if (roll_dot < - SGD_DEGREES_TO_RADIANS * 180) - roll_dot += SGD_DEGREES_TO_RADIANS * 360; - if (roll_dot > SGD_DEGREES_TO_RADIANS * 180) - roll_dot -= SGD_DEGREES_TO_RADIANS * 360; - /*cout << "euler derivatives = " - << roll_dot << " " << pitch_dot << " " << hdg_dot << endl;*/ - - // Now Compute the rotation vector in the carriers coordinate frame - // originating from the euler angle changes. - sgdVec3 body; - body[0] = roll_dot - hdg_dot*sin_pitch; - body[1] = pitch_dot*cos_roll + hdg_dot*sin_roll*cos_pitch; - body[2] = -pitch_dot*sin_roll + hdg_dot*cos_roll*cos_pitch; - - // Transform that back to the horizontal local frame. - sgdVec3 hl; - hl[0] = body[0]*trans[0][0] + body[1]*trans[0][1] + body[2]*trans[0][2]; - hl[1] = body[0]*trans[1][0] + body[1]*trans[1][1] + body[2]*trans[1][2]; - hl[2] = body[0]*trans[2][0] + body[1]*trans[2][1] + body[2]*trans[2][2]; - - // Now we need to project out rotation components ending in speeds in y - // direction in the horizontal local frame. - hl[1] = 0; - - // Transform that to the earth centered frame. - sgdSetVec3(rot_wrt_earth, - - cos_lon*sin_lat*hl[0] - sin_lon*hl[1] - cos_lat*cos_lon*hl[2], - - sin_lon*sin_lat*hl[0] + cos_lon*hl[1] - cos_lat*sin_lon*hl[2], - cos_lat*hl[0] - sin_lat*hl[2]); - } - - UpdateWind(dt); - UpdateFlols(trans); - UpdateElevator(dt, transition_time); -} //end update + UpdateWind(dt); + UpdateElevator(dt, transition_time); + UpdateJBD(dt, jbd_transition_time); + + // Transform that one to the horizontal local coordinate system. + SGQuatd ec2hl = SGQuatd::fromLonLat(pos); + // The orientation of the carrier wrt the horizontal local frame + SGQuatd hl2body = SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); + // and postrotate the orientation of the AIModel wrt the horizontal + // local frame + SGQuatd ec2body = ec2hl*hl2body; + // The cartesian position of the carrier in the wgs84 world + SGVec3d cartPos = SGVec3d::fromGeod(pos); + + // The position of the eyepoint - at least near that ... + SGVec3d eyePos(globals->get_current_view()->get_view_pos()); + // Add the position offset of the AIModel to gain the earth + // centered position + SGVec3d eyeWrtCarrier = eyePos - cartPos; + // rotate the eyepoint wrt carrier vector into the carriers frame + eyeWrtCarrier = ec2body.transform(eyeWrtCarrier); + // the eyepoints vector wrt the flols position + SGVec3d eyeWrtFlols = eyeWrtCarrier - flols_off; + + // the distance from the eyepoint to the flols + dist = norm(eyeWrtFlols); + + // now the angle, positive angles are upwards + if (fabs(dist) < SGLimits::min()) { + angle = 0; + } else { + double sAngle = -eyeWrtFlols(2)/dist; + sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle)); + angle = SGMiscd::rad2deg(asin(sAngle)); + } + // set the value of source + if ( angle <= 4.35 && angle > 4.01 ) + source = 1; + else if ( angle <= 4.01 && angle > 3.670 ) + source = 2; + else if ( angle <= 3.670 && angle > 3.330 ) + source = 3; + else if ( angle <= 3.330 && angle > 2.990 ) + source = 4; + else if ( angle <= 2.990 && angle > 2.650 ) + source = 5; + else if ( angle <= 2.650 ) + source = 6; + else + source = 0; +} //end update -bool FGAICarrier::init() { - if (!FGAIShip::init()) +bool FGAICarrier::init(bool search_in_AI_path) { + if (!FGAIShip::init(search_in_AI_path)) return false; - // process the 3d model here - // mark some objects solid, mark the wires ... - - // The model should be used for altitude computations. - // To avoid that every detail in a carrier 3D model will end into - // the aircraft local cache, only set the HOT traversal bit on - // selected objects. - ssgEntity *sel = aip.getSceneGraph(); - // Clear the HOT traversal flag - mark_nohot(sel); - // Selectively set that flag again for wires/cats/solid objects. - // Attach a pointer to this carrier class to those objects. - mark_wires(sel, wire_objects); - mark_cat(sel, catapult_objects); - mark_solid(sel, solid_objects); - _longitude_node = fgGetNode("/position/longitude-deg", true); _latitude_node = fgGetNode("/position/latitude-deg", true); _altitude_node = fgGetNode("/position/altitude-ft", true); - // _elevator_node = fgGetNode("/controls/elevators", true); + + _launchbar_state_node = fgGetNode("/gear/launchbar/state", true); _surface_wind_from_deg_node = fgGetNode("/environment/config/boundary/entry[0]/wind-from-heading-deg", true); @@ -288,18 +220,23 @@ bool FGAICarrier::init() { turn_to_launch_hdg = false; + turn_to_recovery_hdg = false; + turn_to_base_course = true; returning = false; + in_to_wind = false; - initialpos = pos; + mOpBoxPos = pos; base_course = hdg; base_speed = speed; - step = 0; - pos_norm = 0; + pos_norm = raw_pos_norm = 0; elevators = false; transition_time = 150; time_constant = 0.005; - + jbd_pos_norm = raw_jbd_pos_norm = 0; + jbd = false ; + jbd_transition_time = 3; + jbd_time_constant = 0.1; return true; } @@ -323,11 +260,13 @@ void FGAICarrier::bind() { props->tie("controls/base-speed-kts", SGRawValuePointer(&base_speed)); props->tie("controls/start-pos-lat-deg", - SGRawValuePointer(&initialpos[1])); + SGRawValueMethods(pos, &SGGeod::getLatitudeDeg)); props->tie("controls/start-pos-long-deg", - SGRawValuePointer(&initialpos[0])); - props->tie("velocities/speed-kts", - SGRawValuePointer(&speed)); + SGRawValueMethods(pos, &SGGeod::getLongitudeDeg)); + props->tie("controls/mp-control", + SGRawValuePointer(&MPControl)); + props->tie("controls/ai-control", + SGRawValuePointer(&AIControl)); props->tie("environment/surface-wind-speed-true-kts", SGRawValuePointer(&wind_speed_kts)); props->tie("environment/surface-wind-from-true-degs", @@ -338,16 +277,31 @@ void FGAICarrier::bind() { SGRawValuePointer(&rel_wind)); props->tie("environment/rel-wind-speed-kts", SGRawValuePointer(&rel_wind_speed_kts)); - props->tie("controls/flols/wave-off-lights", - SGRawValuePointer(&wave_off_lights)); + props->tie("environment/in-to-wind", + SGRawValuePointer(&in_to_wind)); + //props->tie("controls/flols/wave-off-lights", + // SGRawValuePointer(&wave_off_lights)); props->tie("controls/elevators", SGRawValuePointer(&elevators)); props->tie("surface-positions/elevators-pos-norm", SGRawValuePointer(&pos_norm)); - props->tie("controls/elevators-trans-time-s", + props->tie("controls/constants/elevators/trans-time-s", SGRawValuePointer(&transition_time)); - props->tie("controls/elevators-time-constant", + props->tie("controls/constants/elevators/time-constant", SGRawValuePointer(&time_constant)); + props->tie("controls/jbd", + SGRawValuePointer(&jbd)); + props->tie("surface-positions/jbd-pos-norm", + SGRawValuePointer(&jbd_pos_norm)); + props->tie("controls/constants/jbd/trans-time-s", + SGRawValuePointer(&jbd_transition_time)); + props->tie("controls/constants/jbd/time-constant", + SGRawValuePointer(&jbd_time_constant)); + props->tie("controls/turn-to-recovery-hdg", + SGRawValuePointer(&turn_to_recovery_hdg)); + props->tie("controls/turn-to-base-course", + SGRawValuePointer(&turn_to_base_course)); + props->setBoolValue("controls/flols/cut-lights", false); props->setBoolValue("controls/flols/wave-off-lights", false); @@ -355,6 +309,8 @@ void FGAICarrier::bind() { props->setBoolValue("controls/crew", false); props->setStringValue("navaids/tacan/channel-ID", TACAN_channel_id.c_str()); props->setStringValue("sign", sign.c_str()); + props->setBoolValue("controls/lighting/deck-lights", false); + props->setDoubleValue("controls/lighting/flood-lights-red-norm", 0); } @@ -366,21 +322,29 @@ void FGAICarrier::unbind() { props->untie("controls/flols/distance-m"); props->untie("controls/flols/angle-degs"); props->untie("controls/turn-to-launch-hdg"); - props->untie("velocities/speed-kts"); props->untie("environment/wind-speed-true-kts"); props->untie("environment/wind-from-true-degs"); props->untie("environment/rel-wind-from-degs"); props->untie("environment/rel-wind-speed-kts"); - props->untie("controls/flols/wave-off-lights"); + props->untie("environment/in-to-wind"); + //props->untie("controls/flols/wave-off-lights"); props->untie("controls/elevators"); props->untie("surface-positions/elevators-pos-norm"); - props->untie("controls/elevators-trans-time-secs"); - props->untie("controls/elevators-time-constant"); + props->untie("controls/constants/elevators/trans-time-secs"); + props->untie("controls/constants/elevators/time-constant"); + props->untie("controls/jbd"); + props->untie("surface-positions/jbd/pos-norm"); + props->untie("controls/constants/jbd/trans-time-s"); + props->untie("controls/jbd-time-constant"); + props->untie("controls/mp-control"); + props->untie("controls/ai-control"); + props->untie("controls/turn-to-recovery-hdg"); + props->untie("controls/turn-to-base-course"); } -bool FGAICarrier::getParkPosition(const string& id, Point3D& geodPos, - double& hdng, sgdVec3 uvw) +bool FGAICarrier::getParkPosition(const string& id, SGGeod& geodPos, + double& hdng, SGVec3d& uvw) { // FIXME: does not yet cover rotation speeds. @@ -389,12 +353,13 @@ bool FGAICarrier::getParkPosition(const string& id, Point3D& geodPos, // Take either the specified one or the first one ... if ((*it).name == id || id.empty()) { ParkPosition ppos = *it; - geodPos = getGeocPosAt(ppos.offset); + SGVec3d cartPos = getCartPosAt(ppos.offset); + geodPos = SGGeod::fromCart(cartPos); hdng = hdg + ppos.heading_deg; double shdng = sin(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); double chdng = cos(ppos.heading_deg * SGD_DEGREES_TO_RADIANS); double speed_fps = speed*1.6878099; - sgdSetVec3(uvw, chdng*speed_fps, shdng*speed_fps, 0); + uvw = SGVec3d(chdng*speed_fps, shdng*speed_fps, 0); return true; } ++it; @@ -403,355 +368,9 @@ bool FGAICarrier::getParkPosition(const string& id, Point3D& geodPos, return false; } - -void FGAICarrier::mark_nohot(ssgEntity* e) { - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - mark_nohot(kid); - - br->clrTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - - e->clrTraversalMaskBits(SSGTRAV_HOT); - } -} - - -bool FGAICarrier::mark_wires(ssgEntity* e, const list& wire_objects, bool mark) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - - list::const_iterator it; - for (it = wire_objects.begin(); it != wire_objects.end(); ++it) - mark = mark || (e->getName() && (*it) == e->getName()); - - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_wires(kid, wire_objects, mark) || found; - - if (found) - br->setTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - list::const_iterator it; - for (it = wire_objects.begin(); it != wire_objects.end(); ++it) { - if (mark || (e->getName() && (*it) == e->getName())) { - e->setTraversalMaskBits(SSGTRAV_HOT); - ssgBase* ud = e->getUserData(); - if (ud) { - FGAICarrierHardware* ch = dynamic_cast(ud); - if (ch) { - SG_LOG(SG_GENERAL, SG_WARN, - "AICarrier: Carrier hardware gets marked twice!\n" - " You have probably a whole branch marked as" - " a wire which also includes other carrier hardware."); - } else { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found user data attached to a leaf node which " - "should be marked as a wire!\n ****Skipping!****"); - } - } else { - e->setUserData( FGAICarrierHardware::newWire( this ) ); - ssgLeaf *l = (ssgLeaf*)e; - if ( l->getNumLines() != 1 ) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found wires not modeled with exactly one line!"); - } - found = true; - } - } - } - } - return found; -} - - -bool FGAICarrier::mark_solid(ssgEntity* e, const list& solid_objects, bool mark) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - - list::const_iterator it; - for (it = solid_objects.begin(); it != solid_objects.end(); ++it) - mark = mark || (e->getName() && (*it) == e->getName()); - - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_solid(kid, solid_objects, mark) || found; - - if (found) - br->setTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - list::const_iterator it; - for (it = solid_objects.begin(); it != solid_objects.end(); ++it) { - if (mark || (e->getName() && (*it) == e->getName())) { - e->setTraversalMaskBits(SSGTRAV_HOT); - ssgBase* ud = e->getUserData(); - - if (ud) { - FGAICarrierHardware* ch = dynamic_cast(ud); - if (ch) { - SG_LOG(SG_GENERAL, SG_WARN, - "AICarrier: Carrier hardware gets marked twice!\n" - " You have probably a whole branch marked solid" - " which also includes other carrier hardware."); - } else { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found user data attached to a leaf node which " - "should be marked solid!\n ****Skipping!****"); - } - } else { - e->setUserData( FGAICarrierHardware::newSolid( this ) ); - found = true; - } - } - } - } - return found; -} - - -bool FGAICarrier::mark_cat(ssgEntity* e, const list& cat_objects, bool mark) { - bool found = false; - if (e->isAKindOf(ssgTypeBranch())) { - ssgBranch* br = (ssgBranch*)e; - ssgEntity* kid; - - list::const_iterator it; - for (it = cat_objects.begin(); it != cat_objects.end(); ++it) - mark = mark || (e->getName() && (*it) == e->getName()); - - for ( kid = br->getKid(0); kid != NULL ; kid = br->getNextKid() ) - found = mark_cat(kid, cat_objects, mark) || found; - - if (found) - br->setTraversalMaskBits(SSGTRAV_HOT); - - } else if (e->isAKindOf(ssgTypeLeaf())) { - list::const_iterator it; - for (it = cat_objects.begin(); it != cat_objects.end(); ++it) { - if (mark || (e->getName() && (*it) == e->getName())) { - e->setTraversalMaskBits(SSGTRAV_HOT); - ssgBase* ud = e->getUserData(); - if (ud) { - FGAICarrierHardware* ch = dynamic_cast(ud); - if (ch) { - SG_LOG(SG_GENERAL, SG_WARN, - "AICarrier: Carrier hardware gets marked twice!\n" - "You have probably a whole branch marked as" - "a catapult which also includes other carrier hardware."); - } else { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found user data attached to a leaf node which " - "should be marked as a catapult!\n ****Skipping!****"); - } - } else { - e->setUserData( FGAICarrierHardware::newCatapult( this ) ); - ssgLeaf *l = (ssgLeaf*)e; - if ( l->getNumLines() != 1 ) { - SG_LOG(SG_GENERAL, SG_ALERT, - "AICarrier: Found a cat not modeled with exactly " - "one line!"); - } else { - // Now some special code to make sure the cat points in the right - // direction. The 0 index must be the backward end, the 1 index - // the forward end. - // Forward is positive x-direction in our 3D model, also the model - // as such is flattened when it is loaded, so we do not need to - // care for transforms ... - short v[2]; - l->getLine(0, v, v+1 ); - sgVec3 ends[2]; - for (int k=0; k<2; ++k) - sgCopyVec3( ends[k], l->getVertex( v[k] ) ); - - // When the 1 end is behind the 0 end, swap the coordinates. - if (ends[0][0] < ends[1][0]) { - sgCopyVec3( l->getVertex( v[0] ), ends[1] ); - sgCopyVec3( l->getVertex( v[1] ), ends[0] ); - } - found = true; - } - } - } - } - } - return found; -} - - -void FGAICarrier::UpdateFlols(const sgdMat3& trans) { - - float in[3]; - float out[3]; - - double flolsXYZ[3], eyeXYZ[3]; - double lat, lon, alt; - Point3D eyepos; - Point3D flolspos; - -/* cout << "x_offset " << flols_x_offset - << " y_offset " << flols_y_offset - << " z_offset " << flols_z_offset << endl; - - cout << "roll " << roll - << " heading " << hdg - << " pitch " << pitch << endl; - - cout << "carrier lon " << pos[0] - << " lat " << pos[1] - << " alt " << pos[2] << endl; -*/ - - // set the Flols initial position to the carrier position - - flolspos = pos; - -/* cout << "flols lon " << flolspos[0] - << " lat " << flolspos[1] - << " alt " << flolspos[2] << endl; - - // set the offsets in metres - - cout << "flols_x_offset " << flols_x_offset << endl - << "flols_y_offset " << flols_y_offset << endl - << "flols_z_offset " << flols_z_offset << endl; -*/ - - in[0] = flols_off.x(); - in[1] = flols_off.y(); - in[2] = flols_off.z(); - - // multiply the input and transform matrices - out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2]; - out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2]; - out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2]; - - // convert meters to ft to degrees of latitude - out[0] = (out[0] * 3.28083989501) / - (366468.96 - 3717.12 * cos(flolspos[0] * SG_DEGREES_TO_RADIANS)); - - // convert meters to ft to degrees of longitude - out[1] = (out[1] * 3.28083989501) / - (365228.16 * cos(flolspos[1] * SG_DEGREES_TO_RADIANS)); - -/* cout << "lat adjust deg" << out[0] - << " lon adjust deg " << out[1] - << " alt adjust m " << out[2] << endl; -*/ - - // adjust Flols position - flolspos[0] += out[0]; - flolspos[1] += out[1]; - flolspos[2] += out[2]; - - // convert flols position to cartesian co-ordinates - sgGeodToCart(flolspos[1] * SG_DEGREES_TO_RADIANS, - flolspos[0] * SG_DEGREES_TO_RADIANS, - flolspos[2] , flolsXYZ ); - - -/* cout << "flols X " << flolsXYZ[0] - << " Y " << flolsXYZ[1] - << " Z " << flolsXYZ[2] << endl; - - // check the conversion - - sgCartToGeod(flolsXYZ, &lat, &lon, &alt); - - cout << "flols check lon " << lon - << " lat " << lat - << " alt " << alt << endl; -*/ - - // get the current position of the pilot's eyepoint (cartesian coordinates) - sgdCopyVec3( eyeXYZ, globals->get_current_view()->get_absolute_view_pos() ); - -/* cout << "Eye_X " << eyeXYZ[0] - << " Eye_Y " << eyeXYZ[1] - << " Eye_Z " << eyeXYZ[2] << endl; -*/ - - sgCartToGeod(eyeXYZ, &lat, &lon, &alt); - - eyepos[0] = lon * SG_RADIANS_TO_DEGREES; - eyepos[1] = lat * SG_RADIANS_TO_DEGREES; - eyepos[2] = alt; - -/* cout << "eye lon " << eyepos[0] - << " eye lat " << eyepos[1] - << " eye alt " << eyepos[2] << endl; -*/ - - //calculate the distance from eye to flols - dist = sgdDistanceVec3( flolsXYZ, eyeXYZ ); - - //apply an index error - dist -= 100; - - //cout << "distance " << dist << endl; - if ( dist > 5000 ) - return; - - // calculate height above FLOLS - double y = eyepos[2] - flolspos[2]; - - // calculate the angle from the flols to eye - // above the horizontal - // double angle; - - if ( dist != 0 ) - angle = asin( y / dist ); - else - angle = 0.0; - - angle *= SG_RADIANS_TO_DEGREES; - - - // cout << " height " << y << " angle " << angle ; - - // set the value of source - - if ( angle <= 4.35 && angle > 4.01 ) - source = 1; - else if ( angle <= 4.01 && angle > 3.670 ) - source = 2; - else if ( angle <= 3.670 && angle > 3.330 ) - source = 3; - else if ( angle <= 3.330 && angle > 2.990 ) - source = 4; - else if ( angle <= 2.990 && angle > 2.650 ) - source = 5; - else if ( angle <= 2.650 ) - source = 6; - else - source = 0; - - // cout << " source " << source << endl; - -} // end updateflols - - - // find relative wind void FGAICarrier::UpdateWind( double dt) { - double recip; - - //calculate the reciprocal hdg - - if (hdg >= 180) - recip = hdg - 180; - else - recip = hdg + 180; - - //cout <<" heading: " << hdg << "recip: " << recip << endl; - //get the surface wind speed and direction wind_from_deg = _surface_wind_from_deg_node->getDoubleValue(); wind_speed_kts = _surface_wind_speed_node->getDoubleValue(); @@ -773,27 +392,21 @@ void FGAICarrier::UpdateWind( double dt) { + (rel_wind_speed_from_north_kts * rel_wind_speed_from_north_kts)); //calculate the relative wind direction - rel_wind_from_deg = atan(rel_wind_speed_from_east_kts/rel_wind_speed_from_north_kts) + rel_wind_from_deg = atan2(rel_wind_speed_from_east_kts, rel_wind_speed_from_north_kts) * SG_RADIANS_TO_DEGREES; - // rationalise the output - if (rel_wind_speed_from_north_kts <= 0) { - rel_wind_from_deg = 180 + rel_wind_from_deg; - } else { - if(rel_wind_speed_from_east_kts <= 0) - rel_wind_from_deg = 360 + rel_wind_from_deg; - } - //calculate rel wind rel_wind = rel_wind_from_deg - hdg; - if (rel_wind > 180) - rel_wind -= 360; + SG_NORMALIZE_RANGE(rel_wind, -180.0, 180.0); + + //set in to wind property + InToWind(); //switch the wave-off lights - if (InToWind()) - wave_off_lights = false; - else - wave_off_lights = true; + //if (InToWind()) + // wave_off_lights = false; + //else + // wave_off_lights = true; // cout << "rel wind: " << rel_wind << endl; @@ -802,18 +415,52 @@ void FGAICarrier::UpdateWind( double dt) { void FGAICarrier::TurnToLaunch(){ + // calculate tgt heading + if (wind_speed_kts < 3){ + tgt_heading = base_course; + } else { + tgt_heading = wind_from_deg; + } + //calculate tgt speed double tgt_speed = 25 - wind_speed_kts; if (tgt_speed < 10) tgt_speed = 10; //turn the carrier - FGAIShip::TurnTo(wind_from_deg); + FGAIShip::TurnTo(tgt_heading); FGAIShip::AccelTo(tgt_speed); } +void FGAICarrier::TurnToRecover(){ + //these are the rules for adjusting heading to provide a relative wind + //down the angled flightdeck + + if (wind_speed_kts < 3){ + tgt_heading = base_course + 60; + } else if (rel_wind < -9 && rel_wind >= -180){ + tgt_heading = wind_from_deg; + } else if (rel_wind > -7 && rel_wind < 45){ + tgt_heading = wind_from_deg + 60; + } else if (rel_wind >=45 && rel_wind < 180){ + tgt_heading = wind_from_deg + 45; + } else + tgt_heading = hdg; + + SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0); + + //calculate tgt speed + double tgt_speed = 26 - wind_speed_kts; + if (tgt_speed < 10) + tgt_speed = 10; + + //turn the carrier + FGAIShip::TurnTo(tgt_heading); + FGAIShip::AccelTo(tgt_speed); + +} void FGAICarrier::TurnToBase(){ //turn the carrier @@ -826,18 +473,8 @@ void FGAICarrier::TurnToBase(){ void FGAICarrier::ReturnToBox(){ double course, distance, az2; - //get the carrier position - carrierpos = pos; - - //cout << "lat: " << carrierpos[1] << " lon: " << carrierpos[0] << endl; - //calculate the bearing and range of the initial position from the carrier - geo_inverse_wgs_84(carrierpos[2], - carrierpos[1], - carrierpos[0], - initialpos[1], - initialpos[0], - &course, &az2, &distance); + geo_inverse_wgs_84(pos, mOpBoxPos, &course, &az2, &distance); distance *= SG_METER_TO_NM; @@ -861,33 +498,33 @@ bool FGAICarrier::OutsideBox() { //returns true if the carrier is outside operat return false; } - if (initialpos[1] >= 0) { //northern hemisphere - if (pos[1] >= initialpos[1] + max_lat) + if (mOpBoxPos.getLatitudeDeg() >= 0) { //northern hemisphere + if (pos.getLatitudeDeg() >= mOpBoxPos.getLatitudeDeg() + max_lat) return true; - if (pos[1] <= initialpos[1] - min_lat) + if (pos.getLatitudeDeg() <= mOpBoxPos.getLatitudeDeg() - min_lat) return true; } else { //southern hemisphere - if (pos[1] <= initialpos[1] - max_lat) + if (pos.getLatitudeDeg() <= mOpBoxPos.getLatitudeDeg() - max_lat) return true; - if (pos[1] >= initialpos[1] + min_lat) + if (pos.getLatitudeDeg() >= mOpBoxPos.getLatitudeDeg() + min_lat) return true; } - if (initialpos[0] >=0) { //eastern hemisphere - if (pos[0] >= initialpos[0] + max_long) + if (mOpBoxPos.getLongitudeDeg() >=0) { //eastern hemisphere + if (pos.getLongitudeDeg() >= mOpBoxPos.getLongitudeDeg() + max_long) return true; - if (pos[0] <= initialpos[0] - min_long) + if (pos.getLongitudeDeg() <= mOpBoxPos.getLongitudeDeg() - min_long) return true; } else { //western hemisphere - if (pos[0] <= initialpos[0] - max_long) + if (pos.getLongitudeDeg() <= mOpBoxPos.getLongitudeDeg() - max_long) return true; - if (pos[0] >= initialpos[0] + min_long) + if (pos.getLongitudeDeg() >= mOpBoxPos.getLongitudeDeg() + min_long) return true; } @@ -897,50 +534,89 @@ bool FGAICarrier::OutsideBox() { //returns true if the carrier is outside operat } // end OutsideBox -// return the distance to the horizon, given the altitude and the radius of the earth -float FGAICarrier::Horizon(float h) { - return RADIUS_M * acos(RADIUS_M / (RADIUS_M + h)); -} - - bool FGAICarrier::InToWind() { - if ( fabs(rel_wind) < 5 ) - return true; + in_to_wind = false; + if ( fabs(rel_wind) < 10 ){ + in_to_wind = true; + return true; + } return false; } void FGAICarrier::UpdateElevator(double dt, double transition_time) { + double step = 0; + if ((elevators && pos_norm >= 1 ) || (!elevators && pos_norm <= 0 )) return; // move the elevators if ( elevators ) { - step += dt/transition_time; + step = dt/transition_time; if ( step > 1 ) step = 1; - } else { - step -= dt/transition_time; - if ( step < 0 ) - step = 0; + step = -dt/transition_time; + if ( step < -1 ) + step = -1; } // assume a linear relationship - raw_pos_norm = step; + raw_pos_norm += step; + + //low pass filter + pos_norm = (raw_pos_norm * time_constant) + (pos_norm * (1 - time_constant)); + + //sanitise the output if (raw_pos_norm >= 1) { raw_pos_norm = 1; } else if (raw_pos_norm <= 0) { raw_pos_norm = 0; } - - //low pass filter - pos_norm = (raw_pos_norm * time_constant) + (pos_norm * (1 - time_constant)); return; } // end UpdateElevator +void FGAICarrier::UpdateJBD(double dt, double jbd_transition_time) { + + string launchbar_state = _launchbar_state_node->getStringValue(); + double step = 0; + + if (launchbar_state == "Engaged"){ + jbd = true; + } else { + jbd = false; + } -int FGAICarrierHardware::unique_id = 1; + if (( jbd && jbd_pos_norm >= 1 ) || ( !jbd && jbd_pos_norm <= 0 )){ + return; + } + + // move the jbds + if ( jbd ) { + step = dt/jbd_transition_time; + if ( step > 1 ) + step = 1; + } else { + step = -dt/jbd_transition_time; + if ( step < -1 ) + step = -1; + } + + // assume a linear relationship + raw_jbd_pos_norm += step; + + //low pass filter + jbd_pos_norm = (raw_jbd_pos_norm * jbd_time_constant) + (jbd_pos_norm * (1 - jbd_time_constant)); + + //sanitise the output + if (jbd_pos_norm >= 1) { + jbd_pos_norm = 1; + } else if (jbd_pos_norm <= 0) { + jbd_pos_norm = 0; + } + + return; +} // end UpdateJBD