Mathias Frhlich:
This patch makes use of the vectors now available in simgear with that past patch. And using that it simplyfies the carrier code somehow. - Small additional factory's to the quaternion code are done in the simgear part. Also more explicit unit names in the factory functions. - The flightgear part makes use of them and simplyfies some computations especially in the carrier code. - The data part fixes the coordinate frames I used for the park positions in the carrier to match the usual ones. I believed that I had done so, but it was definitly different. Also there are more parking positions avaliable now.
This commit is contained in:
ehofman
parent
f928df6caf
commit
5fe860750e
10 changed files with 138 additions and 310 deletions
|
|
@ -335,35 +335,23 @@ double FGAIBase::UpdateRadar(FGAIManager* manager)
|
|||
return range_ft2;
|
||||
}
|
||||
|
||||
Point3D
|
||||
FGAIBase::getCartPosAt(const Point3D& off) const
|
||||
SGVec3d
|
||||
FGAIBase::getCartPosAt(const SGVec3d& _off) const
|
||||
{
|
||||
// The offset converted to the usual body fixed coordinate system.
|
||||
sgdVec3 sgdOff;
|
||||
sgdSetVec3(sgdOff, -off.x(), off.z(), -off.y());
|
||||
|
||||
// Transform that one to the horizontal local coordinate system.
|
||||
sgdMat4 hlTrans;
|
||||
sgdMakeRotMat4(hlTrans, hdg, pitch, roll);
|
||||
sgdXformPnt3(sgdOff, hlTrans);
|
||||
SGQuatd hlTrans = SGQuatd::fromLonLatDeg(pos.lon(), pos.lat());
|
||||
// and postrotate the orientation of the AIModel wrt the horizontal
|
||||
// local frame
|
||||
hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
|
||||
|
||||
// Now transform to the wgs84 earth centeres system.
|
||||
Point3D pos2(pos.lon()* SGD_DEGREES_TO_RADIANS,
|
||||
pos.lat() * SGD_DEGREES_TO_RADIANS,
|
||||
pos.elev());
|
||||
Point3D cartPos3D = sgGeodToCart(pos2);
|
||||
sgdMat4 ecTrans;
|
||||
sgdMakeCoordMat4(ecTrans, cartPos3D.x(), cartPos3D.y(), cartPos3D.z(),
|
||||
pos.lon(), 0, - 90 - pos.lat());
|
||||
sgdXformPnt3(sgdOff, ecTrans);
|
||||
// The offset converted to the usual body fixed coordinate system
|
||||
// rotated to the earth fiexed coordinates axis
|
||||
SGVec3d off = hlTrans.backTransform(_off);
|
||||
|
||||
return Point3D(sgdOff[0], sgdOff[1], sgdOff[2]);
|
||||
}
|
||||
// Add the position offset of the AIModel to gain the earth centered position
|
||||
SGVec3d cartPos = SGGeod::fromDegFt(pos.lon(), pos.lat(), pos.elev());
|
||||
|
||||
Point3D
|
||||
FGAIBase::getGeocPosAt(const Point3D& off) const
|
||||
{
|
||||
return sgCartToGeod(getCartPosAt(off));
|
||||
return cartPos + off;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <list>
|
||||
|
||||
#include <simgear/constants.h>
|
||||
#include <simgear/math/SGMath.hxx>
|
||||
#include <simgear/math/point3d.hxx>
|
||||
#include <simgear/scene/model/placement.hxx>
|
||||
#include <simgear/misc/sg_path.hxx>
|
||||
|
|
@ -75,8 +76,7 @@ public:
|
|||
void setDie( bool die );
|
||||
bool getDie();
|
||||
|
||||
Point3D getCartPosAt(const Point3D& off) const;
|
||||
Point3D getGeocPosAt(const Point3D& off) const;
|
||||
SGVec3d getCartPosAt(const SGVec3d& off) const;
|
||||
|
||||
protected:
|
||||
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <simgear/math/SGMath.hxx>
|
||||
#include <simgear/math/point3d.hxx>
|
||||
#include <simgear/math/sg_geodesy.hxx>
|
||||
#include <math.h>
|
||||
|
|
@ -61,11 +62,14 @@ void FGAICarrier::readFromScenario(SGPropertyNode* scFileNode) {
|
|||
|
||||
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<SGPropertyNode_ptr> props = scFileNode->getChildren("wire");
|
||||
std::vector<SGPropertyNode_ptr>::const_iterator it;
|
||||
|
|
@ -92,11 +96,14 @@ void FGAICarrier::readFromScenario(SGPropertyNode* scFileNode) {
|
|||
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);
|
||||
}
|
||||
}
|
||||
|
|
@ -134,57 +141,32 @@ void FGAICarrier::setTACANChannelID(const string& 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 );
|
||||
sgdCopyVec3(v, vel_wrt_earth.sg() );
|
||||
sgdCopyVec3(omega, rot_wrt_earth.sg() );
|
||||
sgdCopyVec3(pivot, rot_pivot_wrt_earth.sg() );
|
||||
}
|
||||
|
||||
void FGAICarrier::update(double dt) {
|
||||
// For computation of rotation speeds we just use finite differences her.
|
||||
// For computation of rotation speeds we just use finite differences here.
|
||||
// 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.
|
||||
// Transform that one to the horizontal local coordinate system.
|
||||
SGQuatd ec2hl = SGQuatd::fromLonLatDeg(pos.lon(), pos.lat());
|
||||
// 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 = SGGeod::fromDegFt(pos.lon(), pos.lat(), pos.elev());
|
||||
// Store for later use by the groundcache
|
||||
rot_pivot_wrt_earth = cartPos;
|
||||
|
||||
// Compute the velocity in m/s in the earth centered coordinate system axis
|
||||
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);
|
||||
vel_wrt_earth = ec2hl.backTransform(SGVec3d(v_north, v_east, 0));
|
||||
|
||||
// Now update the position and heading. This will compute new hdg and
|
||||
// roll values required for the rotation speed computation.
|
||||
|
|
@ -202,60 +184,74 @@ void FGAICarrier::update(double dt) {
|
|||
|
||||
// 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);
|
||||
rot_wrt_earth = SGVec3d::zeros();
|
||||
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 here is the finite difference ...
|
||||
|
||||
// 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 one to the horizontal local coordinate system.
|
||||
SGQuatd ec2hlNew = SGQuatd::fromLonLatDeg(pos.lon(), pos.lat());
|
||||
// compute the new orientation
|
||||
SGQuatd hl2bodyNew = SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
|
||||
// The rotation difference
|
||||
SGQuatd dOr = inverse(ec2body)*ec2hlNew*hl2bodyNew;
|
||||
SGVec3d dOrAngleAxis;
|
||||
dOr.getAngleAxis(dOrAngleAxis);
|
||||
// divided by the time difference provides a rotation speed vector
|
||||
dOrAngleAxis /= dt;
|
||||
|
||||
// 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 rotate the rotation speed vector back into the
|
||||
// earth centered frames coordinates
|
||||
dOrAngleAxis = ec2body.backTransform(dOrAngleAxis);
|
||||
// dOrAngleAxis = hl2body.backTransform(dOrAngleAxis);
|
||||
// dOrAngleAxis(1) = 0;
|
||||
// dOrAngleAxis = ec2hl.backTransform(dOrAngleAxis);
|
||||
rot_wrt_earth = dOrAngleAxis;
|
||||
}
|
||||
|
||||
// Now we need to project out rotation components ending in speeds in y
|
||||
// direction in the horizontal local frame.
|
||||
hl[1] = 0;
|
||||
UpdateWind(dt);
|
||||
UpdateElevator(dt, transition_time);
|
||||
|
||||
// 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]);
|
||||
}
|
||||
// For the flols reuse some computations done above ...
|
||||
|
||||
UpdateWind(dt);
|
||||
UpdateFlols(trans);
|
||||
UpdateElevator(dt, transition_time);
|
||||
// The position of the eyepoint - at least near that ...
|
||||
SGVec3d eyePos(globals->get_current_view()->get_absolute_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<float>::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())
|
||||
return false;
|
||||
|
|
@ -379,8 +375,8 @@ void FGAICarrier::unbind() {
|
|||
}
|
||||
|
||||
|
||||
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 +385,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 = 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;
|
||||
|
|
@ -584,160 +581,6 @@ bool FGAICarrier::mark_cat(ssgEntity* e, const list<string>& cat_objects, bool m
|
|||
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) {
|
||||
|
||||
|
|
|
|||
|
|
@ -87,7 +87,6 @@ public:
|
|||
void getVelocityWrtEarth(sgdVec3& v, sgdVec3& omega, sgdVec3& pivot);
|
||||
virtual void bind();
|
||||
virtual void unbind();
|
||||
void UpdateFlols ( const sgdMat3& trans );
|
||||
void UpdateWind ( double dt );
|
||||
void setWind_from_east( double fps );
|
||||
void setWind_from_north( double fps );
|
||||
|
|
@ -105,8 +104,8 @@ public:
|
|||
|
||||
virtual const char* getTypeString(void) const { return "carrier"; }
|
||||
|
||||
bool getParkPosition(const string& id, Point3D& geodPos,
|
||||
double& hdng, sgdVec3 uvw);
|
||||
bool getParkPosition(const string& id, SGGeod& geodPos,
|
||||
double& hdng, SGVec3d& uvw);
|
||||
|
||||
private:
|
||||
/// Is sufficient to be private, stores a possible position to place an
|
||||
|
|
@ -115,11 +114,11 @@ private:
|
|||
ParkPosition(const ParkPosition& pp)
|
||||
: name(pp.name), offset(pp.offset), heading_deg(pp.heading_deg)
|
||||
{}
|
||||
ParkPosition(const string& n, const Point3D& off = Point3D(), double heading = 0)
|
||||
ParkPosition(const string& n, const SGVec3d& off = SGVec3d(), double heading = 0)
|
||||
: name(n), offset(off), heading_deg(heading)
|
||||
{}
|
||||
string name;
|
||||
Point3D offset;
|
||||
SGVec3d offset;
|
||||
double heading_deg;
|
||||
};
|
||||
|
||||
|
|
@ -143,13 +142,12 @@ private:
|
|||
string sign; // The sign of this carrier.
|
||||
|
||||
// Velocity wrt earth.
|
||||
sgdVec3 vel_wrt_earth;
|
||||
sgdVec3 rot_wrt_earth;
|
||||
sgdVec3 rot_pivot_wrt_earth;
|
||||
|
||||
SGVec3d vel_wrt_earth;
|
||||
SGVec3d rot_wrt_earth;
|
||||
SGVec3d rot_pivot_wrt_earth;
|
||||
|
||||
// these describe the flols
|
||||
Point3D flols_off;
|
||||
SGVec3d flols_off;
|
||||
|
||||
double dist; // the distance of the eyepoint from the flols
|
||||
double angle;
|
||||
|
|
|
|||
|
|
@ -269,8 +269,7 @@ void FGAIManager::setModel(const string& path, ssgBranch *model)
|
|||
}
|
||||
|
||||
bool FGAIManager::getStartPosition(const string& id, const string& pid,
|
||||
Point3D& geodPos, double& heading,
|
||||
sgdVec3 uvw)
|
||||
SGGeod& geodPos, double& hdng, SGVec3d& uvw)
|
||||
{
|
||||
bool found = false;
|
||||
SGPropertyNode* root = fgGetNode("sim/ai", true);
|
||||
|
|
@ -294,7 +293,7 @@ bool FGAIManager::getStartPosition(const string& id, const string& pid,
|
|||
SGSharedPtr<FGAICarrier> carrier = new FGAICarrier;
|
||||
carrier->readFromScenario(scEntry);
|
||||
|
||||
if (carrier->getParkPosition(pid, geodPos, heading, uvw)) {
|
||||
if (carrier->getParkPosition(pid, geodPos, hdng, uvw)) {
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -105,7 +105,7 @@ public:
|
|||
static SGPropertyNode_ptr loadScenarioFile(const std::string& filename);
|
||||
|
||||
static bool getStartPosition(const string& id, const string& pid,
|
||||
Point3D& geodPos, double& hdng, sgdVec3 uvw);
|
||||
SGGeod& geodPos, double& hdng, SGVec3d& uvw);
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -280,8 +280,8 @@ void FGAIMultiplayer::update(double dt)
|
|||
|
||||
// The quaternion rotating from the earth centered frame to the
|
||||
// horizontal local frame
|
||||
SGQuatf qEc2Hl = SGQuatf::fromLonLat((float)geod.getLongitudeRad(),
|
||||
(float)geod.getLatitudeRad());
|
||||
SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)geod.getLongitudeRad(),
|
||||
(float)geod.getLatitudeRad());
|
||||
// The orientation wrt the horizontal local frame
|
||||
SGQuatf hlOr = conj(qEc2Hl)*ecOrient;
|
||||
float hDeg, pDeg, rDeg;
|
||||
|
|
|
|||
|
|
@ -225,8 +225,8 @@ void YASim::update(double dt)
|
|||
void YASim::copyToYASim(bool copyState)
|
||||
{
|
||||
// Physical state
|
||||
float lat = get_Latitude();
|
||||
float lon = get_Longitude();
|
||||
double lat = get_Latitude();
|
||||
double lon = get_Longitude();
|
||||
float alt = get_Altitude() * FT2M;
|
||||
float roll = get_Phi();
|
||||
float pitch = get_Theta();
|
||||
|
|
|
|||
|
|
@ -935,13 +935,13 @@ static bool fgSetPosFromNAV( const string& id, const double& freq ) {
|
|||
static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) {
|
||||
|
||||
// set initial position from runway and heading
|
||||
Point3D geodPos;
|
||||
SGGeod geodPos;
|
||||
double heading;
|
||||
sgdVec3 uvw;
|
||||
SGVec3d uvw;
|
||||
if (FGAIManager::getStartPosition(carrier, posid, geodPos, heading, uvw)) {
|
||||
double lon = geodPos.lon() * SGD_RADIANS_TO_DEGREES;
|
||||
double lat = geodPos.lat() * SGD_RADIANS_TO_DEGREES;
|
||||
double alt = geodPos.elev() * SG_METER_TO_FEET;
|
||||
double lon = geodPos.getLongitudeDeg();
|
||||
double lat = geodPos.getLatitudeDeg();
|
||||
double alt = geodPos.getElevationFt();
|
||||
|
||||
SG_LOG( SG_GENERAL, SG_INFO, "Attempting to set starting position for "
|
||||
<< carrier << " at lat = " << lat << ", lon = " << lon
|
||||
|
|
@ -957,12 +957,12 @@ static bool fgSetPosFromCarrier( const string& carrier, const string& posid ) {
|
|||
fgSetDouble("/orientation/heading-deg", heading);
|
||||
|
||||
fgSetString("/sim/presets/speed-set", "UVW");
|
||||
fgSetDouble("/velocities/uBody-fps", uvw[0]);
|
||||
fgSetDouble("/velocities/vBody-fps", uvw[1]);
|
||||
fgSetDouble("/velocities/wBody-fps", uvw[2]);
|
||||
fgSetDouble("/sim/presets/uBody-fps", uvw[0]);
|
||||
fgSetDouble("/sim/presets/vBody-fps", uvw[1]);
|
||||
fgSetDouble("/sim/presets/wBody-fps", uvw[2]);
|
||||
fgSetDouble("/velocities/uBody-fps", uvw(0));
|
||||
fgSetDouble("/velocities/vBody-fps", uvw(1));
|
||||
fgSetDouble("/velocities/wBody-fps", uvw(2));
|
||||
fgSetDouble("/sim/presets/uBody-fps", uvw(0));
|
||||
fgSetDouble("/sim/presets/vBody-fps", uvw(1));
|
||||
fgSetDouble("/sim/presets/wBody-fps", uvw(2));
|
||||
|
||||
fgSetBool("/sim/presets/onground", true);
|
||||
|
||||
|
|
|
|||
|
|
@ -160,7 +160,7 @@ bool FGMultiplay::process() {
|
|||
|
||||
// The quaternion rotating from the earth centered frame to the
|
||||
// horizontal local frame
|
||||
SGQuatf qEc2Hl = SGQuatf::fromLonLat((float)lon, (float)lat);
|
||||
SGQuatf qEc2Hl = SGQuatf::fromLonLatRad((float)lon, (float)lat);
|
||||
// The orientation wrt the horizontal local frame
|
||||
float heading = ifce->get_Psi();
|
||||
float pitch = ifce->get_Theta();
|
||||
|
|
|
|||
Loading…
Reference in a new issue