void AttitudeReference::PostStep (double simt, double simdt, double mjd) { valid_axes = false; valid_euler = false; valid_tgteuler = false; if (mode >= 4 && tgtmode == 3) { NAVHANDLE hNav = v->GetNavSource (navid); if (hNav) { VECTOR3 tvel,svel; v->GetGlobalVel (svel); NAVDATA data; OBJHANDLE hObj = NULL; oapiGetNavData (hNav, &data); switch (data.type) { case TRANSMITTER_IDS: hObj = data.ids.hVessel; break; case TRANSMITTER_XPDR: hObj = data.xpdr.hVessel; break; case TRANSMITTER_VTOL: hObj = data.vtol.hBase; break; case TRANSMITTER_VOR: { hObj = data.vor.hPlanet; MATRIX3 Rp; oapiGetRotationMatrix (hObj, &Rp); oapiGetGlobalVel (hObj, &tvel); tvel += mul (Rp, _V(-sin(data.vor.lng),0,cos(data.vor.lng)) * PI2/oapiGetPlanetPeriod(hObj)*oapiGetSize(hObj)*cos(data.vor.lat)); tgt_rvel = svel-tvel; sprintf (oapiDebugString(), "rvel: x=%f, y=%f, z=%f", tgt_rvel.x, tgt_rvel.y, tgt_rvel.z); } return; // done } if (hObj) { oapiGetGlobalVel (hObj, &tvel); tgt_rvel = svel-tvel; } else { // TODO } } } }
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; }