bool AttitudeReference::GetTgtEulerAngles (VECTOR3 &tgt_euler) const
{
if (!valid_tgteuler) {
switch (tgtmode) {
case 1: // fixed
tgteuler.y = tgteuler.z = 0.0;
tgteuler += tgt_offs;
have_tgteuler = true;
break;
case 2: // direction of NAV source
case 3: { // relative velocity of NAV source
NAVHANDLE hNav;
if (mode >= 4 && (hNav = v->GetNavSource (navid))) {
VECTOR3 dir, sdir;
if (tgtmode == 2) {
oapiGetNavPos (hNav, &dir);
v->GetGlobalPos (sdir);
dir = tmul (GetFrameRotMatrix(), unit (dir-sdir));
} else {
v->GetGlobalVel (sdir);
dir = tmul (GetFrameRotMatrix(), unit (tgt_rvel));
}
if (projmode == 0) {
tgteuler.y = asin (dir.y);
tgteuler.z = atan2 (dir.x, dir.z);
} else {
tgteuler.y = atan2 (dir.y, dir.z);
tgteuler.z = asin (dir.x);
}
tgteuler += tgt_offs;
//tgteuler += GetEulerAngles (projection_mode);
// frame-relative --> ship-relative; is this a good idea?
tgteuler.y = posangle(tgteuler.y);
tgteuler.z = posangle(tgteuler.z);
have_tgteuler = true;
} else
have_tgteuler = false;
} break;
default:
have_tgteuler = false;
break;
}
}
valid_tgteuler = true;
tgt_euler = tgteuler;
return have_tgteuler;;
}
bool InstrHSI::Redraw2D (SURFHANDLE surf)
{
const float horzx = (float)(texw-312), horzy = (float)(texh-252);
DWORD tp;
int i, j, vofs;
double c, sinc, cosc, brg, slope;
double yaw = vessel->GetYaw(); if (yaw < 0.0) yaw += PI2;
double siny = sin(yaw), cosy = cos(yaw);
dev = 0.0;
NAVHANDLE nv = vessel->GetNavSource (0);
if (nv) {
tp = oapiGetNavType(nv);
if (tp != TRANSMITTER_VOR && tp != TRANSMITTER_ILS)
nv = NULL;
}
if (nv != nav) {
if (nav = nv) {
navRef = vessel->GetSurfaceRef();
navType = tp;
if (navRef) {
VECTOR3 npos;
NAVDATA data;
double rad;
oapiGetNavPos (nav, &npos);
oapiGlobalToEqu (navRef, npos, &navlng, &navlat, &rad);
oapiGetNavData (nav, &data);
if (navType == TRANSMITTER_ILS) crs = data.ils.appdir;
} else nav = NULL;
} else {
navType = TRANSMITTER_NONE;
}
// transform glideslope background
static float gs_tv[4] = {(horzy+171.5f)/(float)texh,(horzy+154.5f)/(float)texh,(horzy+171.5f)/(float)texh,(horzy+154.5f)/(float)texh};
vofs = vtxofs+4;
for (i = 0; i < 4; i++)
grp->Vtx[vofs+i].tv = (navType == TRANSMITTER_ILS ? gs_tv[i] : (horzy+154.5f)/(float)texh);
// transform glideslope indicator
if (navType != TRANSMITTER_ILS) {
vofs = vtxofs+8;
for (i = 0; i < 4; i++) grp->Vtx[vofs+i].y = ycnt-64;
}
}
if (nav) {
double vlng, vlat, vrad, adist;
OBJHANDLE hRef = vessel->GetEquPos (vlng, vlat, vrad);
if (hRef && hRef == navRef) {
Orthodome (vlng, vlat, navlng, navlat, adist, brg);
adist *= oapiGetSize (hRef);
dev = brg-crs;
if (dev < -PI) dev += PI2;
else if (dev >= PI) dev -= PI2;
if (dev < -PI05) dev = -PI-dev;
else if (dev >= PI05) dev = PI-dev;
// calculate slope
if (navType == TRANSMITTER_ILS) {
double s = adist * cos(crs-brg);
double alt = vessel->GetAltitude();
slope = atan2 (alt, s) * DEG;
// transform glideslope indicator
const double tgtslope = 4.0;
double dslope = slope - tgtslope;
float yshift = (float)min(fabs(dslope)*20.0,45.0);
if (dslope < 0.0) yshift = -yshift;
static float gs_y[4] = {ycnt-4.0f, ycnt-4.0f, ycnt+4.0f, ycnt+4.0f};
vofs = vtxofs+8;
for (i = 0; i < 4; i++)
grp->Vtx[vofs+i].y = gs_y[i]+yshift;
}
}
}
static double xp[4] = {-60.5,60.5,-60.5,60.5};
static double yp[4] = {-60.5,-60.5,60.5,60.5};
// transform compass rose
vofs = vtxofs;
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(cosy*xp[i] + siny*yp[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-siny*xp[i] + cosy*yp[i] + ycnt);
}
// transform source bearing indicator
vofs = vtxofs+12;
if (nav) {
c = yaw-brg;
sinc = sin(c), cosc = cos(c);
static double xs[4] = {-6.2,6.2,-6.2,6.2};
static double ys[4] = {-61,-61,-45,-45};
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(cosc*xs[i] + sinc*ys[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-sinc*xs[i] + cosc*ys[i] + ycnt);
}
} else { // hide indicator
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(xcnt-65.0);
grp->Vtx[i+vofs].y = (float)ycnt;
}
}
// transform course indicator + scale
c = yaw-crs;
sinc = sin(c), cosc = cos(c);
static double xc[8] = {-32.2,32.2,-32.2,32.2, -6.2, 6.2, -6.2, 6.2};
static double yc[8] = { -4.7, -4.7, 4.7, 4.7,-60.5,-60.5,60.5,60.5};
vofs = vtxofs+16;
for (i = 0; i < 8; i++) {
grp->Vtx[i+vofs].x = (float)(cosc*xc[i] + sinc*yc[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-sinc*xc[i] + cosc*yc[i] + ycnt);
}
// transform deviation indicator
static double xd[4] = {-3.65,3.65,-3.65,3.65};
static double yd[4] = {-26.82,-26.82,26.82,26.82};
double dx = min(8.0,fabs(dev)*DEG)*5.175;
if (dev < 0.0) dx = -dx;
vofs = vtxofs+24;
for (i = 0; i < 4; i++) {
grp->Vtx[i+vofs].x = (float)(cosc*(xd[i]+dx) + sinc*yd[i] + xcnt);
grp->Vtx[i+vofs].y = (float)(-sinc*(xd[i]+dx) + cosc*yd[i] + ycnt);
}
// course readout
int icrs = (int)(crs*DEG+0.5) % 360;
char *cc, cbuf[16];
sprintf (cbuf, "%03d", icrs);
vofs = vtxofs+32;
static double numw = 10.0, num_ofs = horzx+1.0;
static double tu_num[4] = {0,numw/texw,0,numw/texw};
for (cc = cbuf, i = 0; i < 3; cc++, i++) {
double x = ((*cc-'0') * numw + num_ofs)/texw;
for (j = 0; j < 4; j++)
grp->Vtx[i*4+j+vofs].tu = (float)(tu_num[j]+x);
}
return false;
}
const MATRIX3 &AttitudeReference::GetFrameRotMatrix () const
{
// Returns rotation matrix for rotation from reference frame to global frame
if (!valid_axes) {
VECTOR3 axis1, axis2, axis3;
switch (mode) {
case 0: // inertial (ecliptic)
axis3 = _V(1,0,0);
axis2 = _V(0,1,0);
break;
case 1: { // inertial (equator)
MATRIX3 R;
oapiGetPlanetObliquityMatrix (v->GetGravityRef(), &R);
//axis3 = _V(R.m13, R.m23, R.m33);
axis3 = _V(R.m11, R.m21, R.m31);
axis2 = _V(R.m12, R.m22, R.m32);
} break;
case 2: { // orbital velocity / orbital momentum vector
OBJHANDLE hRef = v->GetGravityRef();
v->GetRelativeVel (hRef, axis3);
axis3 = unit (axis3);
VECTOR3 vv, vm;
v->GetRelativePos (hRef, vv); // local vertical
vm = crossp (axis3,vv); // direction of orbital momentum
axis2 = unit (crossp (vm,axis3));
} break;
case 3: { // local horizon / local north (surface)
OBJHANDLE hRef = v->GetSurfaceRef();
v->GetRelativePos (hRef, axis2);
axis2 = unit (axis2);
MATRIX3 prot;
oapiGetRotationMatrix (hRef, &prot);
VECTOR3 paxis = {prot.m12, prot.m22, prot.m32}; // planet rotation axis in global frame
VECTOR3 yaxis = unit (crossp (paxis,axis2)); // direction of yaw=+90 pole in global frame
axis3 = crossp (axis2,yaxis);
} break;
case 4: { // synced to NAV source (type-specific)
NAVDATA ndata;
NAVHANDLE hNav = v->GetNavSource (navid);
axis3 = _V(0,0,1);
axis2 = _V(0,1,0);
if (hNav) {
oapiGetNavData (hNav, &ndata);
switch (ndata.type) {
case TRANSMITTER_IDS: {
VECTOR3 pos, dir, rot;
MATRIX3 R;
VESSEL *vtgt = oapiGetVesselInterface (ndata.ids.hVessel);
vtgt->GetRotationMatrix (R);
vtgt->GetDockParams (ndata.ids.hDock, pos, dir, rot);
axis3 = -mul(R,dir);
axis2 = mul(R,rot);
} break;
case TRANSMITTER_VTOL:
case TRANSMITTER_VOR: {
OBJHANDLE hRef = v->GetSurfaceRef();
VECTOR3 spos, npos;
v->GetRelativePos (hRef, axis2);
v->GetGlobalPos (spos);
axis2 = unit (axis2);
oapiGetNavPos (hNav, &npos);
npos -= spos;
axis3 = unit(crossp(crossp(axis2,npos),axis2));
} break;
}
}
} break;
}
axis1 = crossp(axis2,axis3);
R = _M(axis1.x, axis2.x, axis3.x, axis1.y, axis2.y, axis3.y, axis1.z, axis2.z, axis3.z);
valid_axes = true;
valid_euler = false;
}
return R;
}
