diff --git a/test/gamepadutils.c b/test/gamepadutils.c index 68d8e448fd..c5295c4c54 100644 --- a/test/gamepadutils.c +++ b/test/gamepadutils.c @@ -101,8 +101,9 @@ static SDL_FPoint ProjectVec3ToRect(const Vector3 *v, const SDL_FRect *rect) float fovScaleX = fovScaleY * aspect; float relZ = cameraZ - v->z; - if (relZ < 0.01f) + if (relZ < 0.01f) { relZ = 0.01f; /* Prevent division by 0 or negative depth */ + } float ndc_x = (v->x / relZ) / fovScaleX; float ndc_y = (v->y / relZ) / fovScaleY; @@ -207,6 +208,39 @@ void DrawGyroDebugCircle(SDL_Renderer *renderer, const Quaternion *orientation, SDL_SetRenderDrawColor(renderer, r, g, b, a); } + +void DrawGyroDebugAxes(SDL_Renderer *renderer, const Quaternion *orientation, const SDL_FRect *bounds) +{ + /* Store current color */ + Uint8 r, g, b, a; + SDL_GetRenderDrawColor(renderer, &r, &g, &b, &a); + + Vector3 origin = { 0.0f, 0.0f, 0.0f }; + + Vector3 right = { 1.0f, 0.0f, 0.0f }; + Vector3 up = { 0.0f, 1.0f, 0.0f }; + Vector3 back = { 0.0f, 0.0f, 1.0f }; + + Vector3 world_right = RotateVectorByQuaternion(&right, orientation); + Vector3 world_up = RotateVectorByQuaternion(&up, orientation); + Vector3 world_back = RotateVectorByQuaternion(&back, orientation); + + SDL_FPoint origin_screen = ProjectVec3ToRect(&origin, bounds); + SDL_FPoint right_screen = ProjectVec3ToRect(&world_right, bounds); + SDL_FPoint up_screen = ProjectVec3ToRect(&world_up, bounds); + SDL_FPoint back_screen = ProjectVec3ToRect(&world_back, bounds); + + SDL_SetRenderDrawColor(renderer, GYRO_COLOR_RED); + SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, right_screen.x, right_screen.y); + SDL_SetRenderDrawColor(renderer, GYRO_COLOR_GREEN); + SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, up_screen.x, up_screen.y); + SDL_SetRenderDrawColor(renderer, GYRO_COLOR_BLUE); + SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, back_screen.x, back_screen.y); + + /* Restore current color */ + SDL_SetRenderDrawColor(renderer, r, g, b, a); +} + void DrawAccelerometerDebugArrow(SDL_Renderer *renderer, const Quaternion *gyro_quaternion, const float *accel_data, const SDL_FRect *bounds) { /* Store current color */ @@ -990,6 +1024,8 @@ struct GyroDisplay /* This part displays extra info from the IMUstate in order to figure out actual polling rates. */ float gyro_drift_solution[3]; int reported_sensor_rate_hz; /*hz - comes from HIDsdl implementation. Could be fixed, platform time, or true sensor time*/ + Uint64 next_reported_sensor_time; /* SDL ticks used to throttle the display */ + int estimated_sensor_rate_hz; /*hz - our estimation of the actual polling rate by observing packets received*/ float euler_displacement_angles[3]; /* pitch, yaw, roll */ Quaternion gyro_quaternion; /* Rotation since startup/reset, comprised of each gyro speed packet times sensor delta time. */ @@ -1009,7 +1045,8 @@ GyroDisplay *CreateGyroDisplay(SDL_Renderer *renderer) SDL_zeroa(ctx->gyro_drift_solution); Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f }; ctx->gyro_quaternion = quat_identity; - + ctx->reported_sensor_rate_hz = 0; + ctx->next_reported_sensor_time = 0; ctx->reset_gyro_button = CreateGamepadButton(renderer, "Reset View"); ctx->calibrate_gyro_button = CreateGamepadButton(renderer, "Recalibrate Drift"); } @@ -1024,7 +1061,6 @@ void SetGyroDisplayArea(GyroDisplay *ctx, const SDL_FRect *area) } SDL_copyp(&ctx->area, area); - /* Place the reset button to the bottom right of the gyro display area.*/ SDL_FRect reset_button_area; reset_button_area.w = SDL_max(MINIMUM_BUTTON_WIDTH, GetGamepadButtonLabelWidth(ctx->reset_gyro_button) + 2 * BUTTON_PADDING); @@ -1340,12 +1376,17 @@ void SetGamepadDisplayIMUValues(GyroDisplay *ctx, float *gyro_drift_solution, fl return; } + const int SENSOR_UPDATE_INTERVAL_MS = 100; + Uint64 now = SDL_GetTicks(); + if (now > ctx->next_reported_sensor_time) { + ctx->estimated_sensor_rate_hz = estimated_sensor_rate_hz; + if (reported_senor_rate_hz != 0) { + ctx->reported_sensor_rate_hz = reported_senor_rate_hz; + } + ctx->next_reported_sensor_time = now + SENSOR_UPDATE_INTERVAL_MS; + } + SDL_memcpy(ctx->gyro_drift_solution, gyro_drift_solution, sizeof(ctx->gyro_drift_solution)); - ctx->estimated_sensor_rate_hz = estimated_sensor_rate_hz; - - if (reported_senor_rate_hz != 0) - ctx->reported_sensor_rate_hz = reported_senor_rate_hz; - SDL_memcpy(ctx->euler_displacement_angles, euler_displacement_angles, sizeof(ctx->euler_displacement_angles)); ctx->gyro_quaternion = *gyro_quaternion; ctx->drift_calibration_progress_frac = drift_calibration_progress_frac; @@ -1637,7 +1678,7 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad) SDLTest_DrawString(ctx->renderer, x + center - SDL_strlen(text) * FONT_CHARACTER_SIZE, y, text); SDL_snprintf(text, sizeof(text), "[%.2f,%.2f,%.2f]%s/s", ctx->gyro_data[0] * RAD_TO_DEG, ctx->gyro_data[1] * RAD_TO_DEG, ctx->gyro_data[2] * RAD_TO_DEG, DEGREE_UTF8); SDLTest_DrawString(ctx->renderer, x + center + 2.0f, y, text); - + /* Display the testcontroller tool's evaluation of drift. This is also useful to get an average rate of turn in calibrated turntable tests. */ if (ctx->gyro_drift_correction_data[0] != 0.0f && ctx->gyro_drift_correction_data[2] != 0.0f && ctx->gyro_drift_correction_data[2] != 0.0f ) @@ -1648,10 +1689,7 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad) SDL_snprintf(text, sizeof(text), "[%.2f,%.2f,%.2f]%s/s", ctx->gyro_drift_correction_data[0] * RAD_TO_DEG, ctx->gyro_drift_correction_data[1] * RAD_TO_DEG, ctx->gyro_drift_correction_data[2] * RAD_TO_DEG, DEGREE_UTF8); SDLTest_DrawString(ctx->renderer, x + center + 2.0f, y, text); } - } - - } } SDL_free(mapping); @@ -1797,7 +1835,6 @@ void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_ /* Set the color based on the drift calibration progress fraction */ SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_GREEN); /* red when too much noise, green when low noise*/ - /* Now draw the bars with the filled, then empty rectangles */ SDL_RenderFillRect(ctx->renderer, &progress_bar_fill); /* draw the filled rectangle*/ SDL_SetRenderDrawColor(ctx->renderer, 100, 100, 100, 255); /* gray box*/ @@ -1823,20 +1860,26 @@ float RenderEulerReadout(GyroDisplay *ctx, GamepadDisplay *gamepad_display ) const float new_line_height = gamepad_display->button_height + 2.0f; float log_gyro_euler_text_x = gyro_calibrate_button_rect.x; + Uint8 r, g, b, a; + SDL_GetRenderDrawColor(ctx->renderer, &r, &g, &b, &a); /* Pitch Readout */ + SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_RED); SDL_snprintf(text, sizeof(text), "Pitch: %6.2f%s", ctx->euler_displacement_angles[0], DEGREE_UTF8); SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text); /* Yaw Readout */ + SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_GREEN); log_y += new_line_height; SDL_snprintf(text, sizeof(text), " Yaw: %6.2f%s", ctx->euler_displacement_angles[1], DEGREE_UTF8); SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text); /* Roll Readout */ + SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_BLUE); log_y += new_line_height; SDL_snprintf(text, sizeof(text), " Roll: %6.2f%s", ctx->euler_displacement_angles[2], DEGREE_UTF8); SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text); + SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a); return log_y + new_line_height; /* Return the next y position for further rendering */ } @@ -1859,6 +1902,9 @@ void RenderGyroGizmo(GyroDisplay *ctx, SDL_Gamepad *gamepad, float top) /* Draw the rotated cube */ DrawGyroDebugCube(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect); + /* Draw positive axes */ + DrawGyroDebugAxes(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect); + /* Overlay the XYZ circles */ DrawGyroDebugCircle(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect); @@ -1906,7 +1952,6 @@ void RenderGyroDisplay(GyroDisplay *ctx, GamepadDisplay *gamepadElements, SDL_Ga if (bHasCachedDriftSolution) { float bottom = RenderEulerReadout(ctx, gamepadElements); RenderGyroGizmo(ctx, gamepad, bottom); - } SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a); } diff --git a/test/testcontroller.c b/test/testcontroller.c index 7602253f86..0530d1a992 100644 --- a/test/testcontroller.c +++ b/test/testcontroller.c @@ -53,62 +53,59 @@ struct Quaternion static Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f }; -Quaternion QuaternionFromEuler(float roll, float pitch, float yaw) +Quaternion QuaternionFromEuler(float pitch, float yaw, float roll) { - Quaternion q; + float cx = SDL_cosf(pitch * 0.5f); + float sx = SDL_sinf(pitch * 0.5f); float cy = SDL_cosf(yaw * 0.5f); float sy = SDL_sinf(yaw * 0.5f); - float cp = SDL_cosf(pitch * 0.5f); - float sp = SDL_sinf(pitch * 0.5f); - float cr = SDL_cosf(roll * 0.5f); - float sr = SDL_sinf(roll * 0.5f); + float cz = SDL_cosf(roll * 0.5f); + float sz = SDL_sinf(roll * 0.5f); - q.w = cr * cp * cy + sr * sp * sy; - q.x = sr * cp * cy - cr * sp * sy; - q.y = cr * sp * cy + sr * cp * sy; - q.z = cr * cp * sy - sr * sp * cy; + Quaternion q; + q.w = cx * cy * cz + sx * sy * sz; + q.x = sx * cy * cz - cx * sy * sz; + q.y = cx * sy * cz + sx * cy * sz; + q.z = cx * cy * sz - sx * sy * cz; return q; } -static void EulerFromQuaternion(Quaternion q, float *roll, float *pitch, float *yaw) +#define RAD_TO_DEG (180.0f / SDL_PI_F) + +/* Decomposes quaternion into Yaw (Y), Pitch (X), Roll (Z) using Y-X-Z order in a left-handed system */ +void QuaternionToYXZ(Quaternion q, float *pitch, float *yaw, float *roll) { - float sinr_cosp = 2.0f * (q.w * q.x + q.y * q.z); - float cosr_cosp = 1.0f - 2.0f * (q.x * q.x + q.y * q.y); - float roll_rad = SDL_atan2f(sinr_cosp, cosr_cosp); + /* Precalculate repeated expressions */ + float qxx = q.x * q.x; + float qyy = q.y * q.y; + float qzz = q.z * q.z; - float sinp = 2.0f * (q.w * q.y - q.z * q.x); - float pitch_rad; - if (SDL_fabsf(sinp) >= 1.0f) { - pitch_rad = SDL_copysignf(SDL_PI_F / 2.0f, sinp); - } else { - pitch_rad = SDL_asinf(sinp); - } + float qxy = q.x * q.y; + float qxz = q.x * q.z; + float qyz = q.y * q.z; + float qwx = q.w * q.x; + float qwy = q.w * q.y; + float qwz = q.w * q.z; - float siny_cosp = 2.0f * (q.w * q.z + q.x * q.y); - float cosy_cosp = 1.0f - 2.0f * (q.y * q.y + q.z * q.z); - float yaw_rad = SDL_atan2f(siny_cosp, cosy_cosp); - - if (roll) - *roll = roll_rad; - if (pitch) - *pitch = pitch_rad; - if (yaw) - *yaw = yaw_rad; -} - -static void EulerDegreesFromQuaternion(Quaternion q, float *pitch, float *yaw, float *roll) -{ - float pitch_rad, yaw_rad, roll_rad; - EulerFromQuaternion(q, &pitch_rad, &yaw_rad, &roll_rad); - if (pitch) { - *pitch = pitch_rad * (180.0f / SDL_PI_F); - } + /* Yaw (around Y) */ if (yaw) { - *yaw = yaw_rad * (180.0f / SDL_PI_F); + *yaw = SDL_atan2f(2.0f * (qwy + qxz), 1.0f - 2.0f * (qyy + qzz)) * RAD_TO_DEG; } + + /* Pitch (around X) */ + float sinp = 2.0f * (qwx - qyz); + if (pitch) { + if (SDL_fabsf(sinp) >= 1.0f) { + *pitch = SDL_copysignf(90.0f, sinp); /* Clamp to avoid domain error */ + } else { + *pitch = SDL_asinf(sinp) * RAD_TO_DEG; + } + } + + /* Roll (around Z) */ if (roll) { - *roll = roll_rad * (180.0f / SDL_PI_F); + *roll = SDL_atan2f(2.0f * (qwz + qxy), 1.0f - 2.0f * (qxx + qzz)) * RAD_TO_DEG; } } @@ -1375,7 +1372,16 @@ static void HandleGamepadGyroEvent(SDL_Event *event) SDL_memcpy(controller->imu_state->gyro_data, event->gsensor.data, sizeof(controller->imu_state->gyro_data)); } +/* Two strategies for evaluating polling rate - one based on a fixed packet count, and one using a fixed time window. + * Smaller values in either will give you a more responsive polling rate estimate, but this may fluctuate more. + * Larger values in either will give you a more stable average but they will require more time to evaluate. + * Generally, wired connections tend to give much more stable + */ +/* #define SDL_USE_FIXED_PACKET_COUNT_FOR_ESTIMATION */ #define SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT 2048 +#define SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_TIME_NS (SDL_NS_PER_SECOND * 2) + + static void EstimatePacketRate() { Uint64 now_ns = SDL_GetTicksNS(); @@ -1384,17 +1390,22 @@ static void EstimatePacketRate() } /* Require a significant sample size before averaging rate. */ +#ifdef SDL_USE_FIXED_PACKET_COUNT_FOR_ESTIMATION if (controller->imu_state->imu_packet_counter >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT) { Uint64 deltatime_ns = now_ns - controller->imu_state->starting_time_stamp_ns; - controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * 1000000000ULL) / deltatime_ns); - } - - /* Flush sampled data after a brief period so that the imu_estimated_sensor_rate value can be read.*/ - if (controller->imu_state->imu_packet_counter >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT * 2) { - controller->imu_state->starting_time_stamp_ns = now_ns; + controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * SDL_NS_PER_SECOND) / deltatime_ns); controller->imu_state->imu_packet_counter = 0; } - ++controller->imu_state->imu_packet_counter; +#else + Uint64 deltatime_ns = now_ns - controller->imu_state->starting_time_stamp_ns; + if (deltatime_ns >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_TIME_NS) { + controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * SDL_NS_PER_SECOND) / deltatime_ns); + controller->imu_state->imu_packet_counter = 0; + } +#endif + else { + ++controller->imu_state->imu_packet_counter; + } } static void UpdateGamepadOrientation( Uint64 delta_time_ns ) @@ -1409,13 +1420,11 @@ static void UpdateGamepadOrientation( Uint64 delta_time_ns ) static void HandleGamepadSensorEvent( SDL_Event* event ) { - if (!controller) { - return; - } + if (!controller) + return; - if (controller->id != event->gsensor.which) { + if (controller->id != event->gsensor.which) return; - } if (event->gsensor.sensor == SDL_SENSOR_GYRO) { HandleGamepadGyroEvent(event); @@ -1428,13 +1437,12 @@ static void HandleGamepadSensorEvent( SDL_Event* event ) accelerometer and gyro events are received before progressing. */ if ( controller->imu_state->accelerometer_packet_number == controller->imu_state->gyro_packet_number ) { - EstimatePacketRate(); Uint64 sensorTimeStampDelta_ns = event->gsensor.sensor_timestamp - controller->imu_state->last_sensor_time_stamp_ns ; UpdateGamepadOrientation(sensorTimeStampDelta_ns); float display_euler_angles[3]; - EulerDegreesFromQuaternion(controller->imu_state->integrated_rotation, &display_euler_angles[0], &display_euler_angles[1], &display_euler_angles[2]); + QuaternionToYXZ(controller->imu_state->integrated_rotation, &display_euler_angles[0], &display_euler_angles[1], &display_euler_angles[2]); float drift_calibration_progress_frac = controller->imu_state->gyro_drift_sample_count / (float)SDL_GAMEPAD_IMU_MIN_GYRO_DRIFT_SAMPLE_COUNT; int reported_polling_rate_hz = sensorTimeStampDelta_ns > 0 ? (int)(SDL_NS_PER_SECOND / sensorTimeStampDelta_ns) : 0; @@ -2073,7 +2081,6 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event) event->gsensor.data[1], event->gsensor.data[2], event->gsensor.sensor_timestamp); - #endif /* VERBOSE_SENSORS */ HandleGamepadSensorEvent(event); break;