/* Plugin to show how to use the "sim/flightmodel/position/q" dataref It also has functions to convert from Quats to HPR and from HPR to Quats Version 1.0.0.1 Intitial Sandy Barbour - 14/11/2005 */ #include #include #include "XPLMPlugin.h" #include "XPLMDisplay.h" #include "XPLMGraphics.h" #include "XPLMProcessing.h" #include "XPLMUtilities.h" #include "XPLMDataAccess.h" const float Pi = 3.14159265358979; const float RadToDeg = 180.0 / Pi; const float DegToRad = 1.0 / RadToDeg; typedef struct _QUATERNION { float w; float x; float y; float z; } QUATERNION; typedef struct _HPR { float Heading; float Pitch; float Roll; } HPR; char Buffer[14][80]; XPLMWindowID gWindow = NULL; XPLMDataRef gqDataRef = NULL; XPLMDataRef gHeadingDataRef = NULL; XPLMDataRef gRollDataRef = NULL; XPLMDataRef gPitchDataRef = NULL; float TestQuaternionsLoopCB(float elapsedMe, float elapsedSim, int counter, void * refcon); void TestQuaternionsDrawWindowCallback( XPLMWindowID inWindowID, void * inRefcon); void TestQuaternionsHandleKeyCallback( XPLMWindowID inWindowID, char inKey, XPLMKeyFlags inFlags, char inVirtualKey, void * inRefcon, int losingFocus); int TestQuaternionsHandleMouseClickCallback( XPLMWindowID inWindowID, int x, int y, XPLMMouseStatus inMouse, void * inRefcon); void QuaternionToHPR(QUATERNION quaternion, HPR *phpr); void HPRToQuaternion(HPR hpr, QUATERNION *pquaternion); //--------------------------------------------------------------------------- PLUGIN_API int XPluginStart( char * outName, char * outSig, char * outDesc) { strcpy(outName, "TestQuaternions"); strcpy(outSig, "SandyBarbour.Example.TestQuaternions"); strcpy(outDesc, "A plug-in that tests Quaternions."); gWindow = XPLMCreateWindow( 50, 600, 200, 430, /* Area of the window. */ 1, /* Start visible. */ TestQuaternionsDrawWindowCallback, /* Callbacks */ TestQuaternionsHandleKeyCallback, TestQuaternionsHandleMouseClickCallback, NULL); /* Refcon - not used. */ XPLMRegisterFlightLoopCallback(TestQuaternionsLoopCB, 1.0, NULL); gqDataRef = XPLMFindDataRef("sim/flightmodel/position/q"); gHeadingDataRef = XPLMFindDataRef("sim/flightmodel/position/psi"); gPitchDataRef = XPLMFindDataRef("sim/flightmodel/position/theta"); gRollDataRef = XPLMFindDataRef("sim/flightmodel/position/phi"); memset(Buffer, 0, sizeof(Buffer)); return 1; } //--------------------------------------------------------------------------- PLUGIN_API void XPluginStop(void) { XPLMDestroyWindow(gWindow); XPLMUnregisterFlightLoopCallback(TestQuaternionsLoopCB, NULL); } //--------------------------------------------------------------------------- PLUGIN_API int XPluginEnable(void) { return 1; } //--------------------------------------------------------------------------- PLUGIN_API void XPluginDisable(void) { } //--------------------------------------------------------------------------- PLUGIN_API void XPluginReceiveMessage(XPLMPluginID inFrom, long inMsg, void * inParam) { } //--------------------------------------------------------------------------- void TestQuaternionsDrawWindowCallback( XPLMWindowID inWindowID, void * inRefcon) { float rgb [] = { 1.0, 1.0, 1.0 }; int l, t, r, b; char buf[512]; XPLMGetWindowGeometry(inWindowID, &l, &t, &r, &b); XPLMDrawTranslucentDarkBox(l, t, r, b); for (int i=0; i<14; i++) XPLMDrawString(rgb, l+10, (t-20) - (10*i), Buffer[i], NULL, xplmFont_Basic); } //--------------------------------------------------------------------------- void TestQuaternionsHandleKeyCallback( XPLMWindowID inWindowID, char inKey, XPLMKeyFlags inFlags, char inVirtualKey, void * inRefcon, int losingFocus) { } //--------------------------------------------------------------------------- int TestQuaternionsHandleMouseClickCallback( XPLMWindowID inWindowID, int x, int y, XPLMMouseStatus inMouse, void * inRefcon) { return 1; } //--------------------------------------------------------------------------- float TestQuaternionsLoopCB(float elapsedMe, float elapsedSim, int counter, void * refcon) { QUATERNION q1, q2; HPR hpr1, hpr2; float FloatVals[4]; int count = XPLMGetDatavf(gqDataRef, FloatVals, 0, 4); q1.w = FloatVals[0]; q1.x = FloatVals[1]; q1.y = FloatVals[2]; q1.z = FloatVals[3]; QuaternionToHPR(q1, &hpr1); int Hdg = (int) hpr1.Heading; Hdg %= 360; hpr1.Heading = (float) Hdg; if (hpr1.Heading < 0) hpr1.Heading += 360; hpr2.Heading = XPLMGetDataf(gHeadingDataRef); if (hpr2.Heading > 180) hpr2.Heading = hpr2.Heading - 360; hpr2.Roll = XPLMGetDataf(gRollDataRef); hpr2.Pitch = XPLMGetDataf(gPitchDataRef); HPRToQuaternion(hpr2, &q2); FloatVals[0] = q2.w; FloatVals[1] = q2.x; FloatVals[2] = q2.y; FloatVals[3] = q2.z; XPLMSetDatavf(gqDataRef, FloatVals, 0, 4); sprintf(Buffer[0], "w1 = %f", q1.w); sprintf(Buffer[1], "x1 = %f", q1.x); sprintf(Buffer[2], "y1 = %f", q1.y); sprintf(Buffer[3], "z1 = %f", q1.z); sprintf(Buffer[4], "Heading1 = %f", hpr1.Heading); sprintf(Buffer[5], "Roll1 = %f", hpr1.Roll); sprintf(Buffer[6], "Pitch1 = %f", hpr1.Pitch); sprintf(Buffer[7], "w2 = %f", q2.w); sprintf(Buffer[8], "x2 = %f", q2.x); sprintf(Buffer[9], "y2 = %f", q2.y); sprintf(Buffer[10], "z2 = %f", q2.z); sprintf(Buffer[11], "Heading2 = %f", hpr2.Heading); sprintf(Buffer[12], "Roll2 = %f", hpr2.Roll); sprintf(Buffer[13], "Pitch2 = %f", hpr2.Pitch); return 0.1; } //--------------------------------------------------------------------------- void QuaternionToHPR(QUATERNION quaternion, HPR *phpr) { double local_w = quaternion.w; double local_x = quaternion.x; double local_y = quaternion.y; double local_z = quaternion.z; double sq_w = local_w * local_w; double sq_x = local_x * local_x; double sq_y = local_y * local_y; double sq_z = local_z * local_z; phpr->Heading = atan2(2.0 * (local_x * local_y + local_z * local_w),(sq_x - sq_y - sq_z + sq_w)) * RadToDeg; phpr->Pitch = asin(-2.0 * (local_x * local_z - local_y * local_w)) * RadToDeg; phpr->Roll = atan2(2.0 * (local_y * local_z + local_x * local_w),(-sq_x - sq_y + sq_z + sq_w)) * RadToDeg; } //--------------------------------------------------------------------------- void HPRToQuaternion(HPR hpr, QUATERNION *pquaternion) { double local_Heading = hpr.Heading * DegToRad; double local_Pitch = hpr.Pitch * DegToRad; double local_Roll = hpr.Roll * DegToRad; double Cosine1 = cos(local_Roll / 2); double Cosine2 = cos(local_Pitch / 2); double Cosine3 = cos(local_Heading / 2); double Sine1 = sin(local_Roll / 2); double Sine2 = sin(local_Pitch / 2); double Sine3 = sin(local_Heading / 2); pquaternion->w = Cosine1 * Cosine2 * Cosine3 + Sine1 * Sine2 * Sine3; pquaternion->x = Sine1 * Cosine2 * Cosine3 - Cosine1 * Sine2 * Sine3; pquaternion->y = Cosine1 * Sine2 * Cosine3 + Sine1 * Cosine2 * Sine3; pquaternion->z = Cosine1 * Cosine2 * Sine3 - Sine1 * Sine2 * Cosine3; } //---------------------------------------------------------------------------