services/camera/libcameraservice/device3/DistortionMapper.cpp - LeafOS-Project/android_frameworks_av - Gitiles

 /*
  * Copyright (C) 2018 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 #define LOG_TAG "Camera3-DistMapper"
 #define ATRACE_TAG ATRACE_TAG_CAMERA
 //#define LOG_NDEBUG 0

 #include <algorithm>
 #include <cmath>

 #include "device3/DistortionMapper.h"
 #include "utils/SessionConfigurationUtilsHost.h"

 namespace android {

 namespace camera3 {


 DistortionMapper::DistortionMapper() {
     initRemappedKeys();
 }

 void DistortionMapper::initRemappedKeys() {
     mRemappedKeys.insert(
             kMeteringRegionsToCorrect.begin(),
             kMeteringRegionsToCorrect.end());
     mRemappedKeys.insert(
             kRectsToCorrect.begin(),
             kRectsToCorrect.end());
     mRemappedKeys.insert(
             kResultPointsToCorrectNoClamp.begin(),
             kResultPointsToCorrectNoClamp.end());
     mRemappedKeys.insert(ANDROID_DISTORTION_CORRECTION_MODE);
 }

 bool DistortionMapper::isDistortionSupported(const CameraMetadata &deviceInfo) {
     bool isDistortionCorrectionSupported = false;
     camera_metadata_ro_entry_t distortionCorrectionModes =
             deviceInfo.find(ANDROID_DISTORTION_CORRECTION_AVAILABLE_MODES);
     for (size_t i = 0; i < distortionCorrectionModes.count; i++) {
         if (distortionCorrectionModes.data.u8[i] !=
                 ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
             isDistortionCorrectionSupported = true;
             break;
         }
     }
     return isDistortionCorrectionSupported;
 }

 status_t DistortionMapper::setupStaticInfo(const CameraMetadata &deviceInfo) {
     std::lock_guard<std::mutex> lock(mMutex);
     status_t res = setupStaticInfoLocked(deviceInfo, /*maxResolution*/false);
     if (res != OK) {
         return res;
     }

     bool mMaxResolution = SessionConfigurationUtils::supportsUltraHighResolutionCapture(deviceInfo);
     if (mMaxResolution) {
         res = setupStaticInfoLocked(deviceInfo, /*maxResolution*/true);
     }
     return res;
 }

 status_t DistortionMapper::setupStaticInfoLocked(const CameraMetadata &deviceInfo,
         bool maxResolution) {
     DistortionMapperInfo *mapperInfo = maxResolution ? &mDistortionMapperInfoMaximumResolution :
             &mDistortionMapperInfo;

     camera_metadata_ro_entry_t array;

     array = deviceInfo.find(
         SessionConfigurationUtils::getAppropriateModeTag(
                 ANDROID_SENSOR_INFO_PRE_CORRECTION_ACTIVE_ARRAY_SIZE, maxResolution));
     if (array.count != 4) return BAD_VALUE;

     float arrayX = static_cast<float>(array.data.i32[0]);
     float arrayY = static_cast<float>(array.data.i32[1]);
     mapperInfo->mArrayWidth = static_cast<float>(array.data.i32[2]);
     mapperInfo->mArrayHeight = static_cast<float>(array.data.i32[3]);

     array = deviceInfo.find(
             SessionConfigurationUtils::getAppropriateModeTag(
                     ANDROID_SENSOR_INFO_ACTIVE_ARRAY_SIZE, maxResolution));
     if (array.count != 4) return BAD_VALUE;

     float activeX = static_cast<float>(array.data.i32[0]);
     float activeY = static_cast<float>(array.data.i32[1]);
     mapperInfo->mActiveWidth = static_cast<float>(array.data.i32[2]);
     mapperInfo->mActiveHeight = static_cast<float>(array.data.i32[3]);

     mapperInfo->mArrayDiffX = activeX - arrayX;
     mapperInfo->mArrayDiffY = activeY - arrayY;

     return updateCalibration(deviceInfo, /*isStatic*/ true, maxResolution);
 }

 static bool doesSettingsHaveMaxResolution(const CameraMetadata *settings) {
     if (settings == nullptr) {
         return false;
     }
     // First we get the sensorPixelMode from the settings metadata.
     camera_metadata_ro_entry sensorPixelModeEntry = settings->find(ANDROID_SENSOR_PIXEL_MODE);
     if (sensorPixelModeEntry.count != 0) {
         return (sensorPixelModeEntry.data.u8[0] == ANDROID_SENSOR_PIXEL_MODE_MAXIMUM_RESOLUTION);
     }
     return false;
 }

 bool DistortionMapper::calibrationValid() const {
     std::lock_guard<std::mutex> lock(mMutex);
     bool isValid =  mDistortionMapperInfo.mValidMapping;
     if (mMaxResolution) {
         isValid = isValid && mDistortionMapperInfoMaximumResolution.mValidMapping;
     }
     return isValid;
 }

 status_t DistortionMapper::correctCaptureRequest(CameraMetadata *request) {
     std::lock_guard<std::mutex> lock(mMutex);
     status_t res;

     bool maxResolution = doesSettingsHaveMaxResolution(request);
     DistortionMapperInfo *mapperInfo = maxResolution ? &mDistortionMapperInfoMaximumResolution :
             &mDistortionMapperInfo;

     if (!mapperInfo->mValidMapping) return OK;

     camera_metadata_entry_t e;
     e = request->find(ANDROID_DISTORTION_CORRECTION_MODE);
     if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
         for (auto region : kMeteringRegionsToCorrect) {
             e = request->find(region);
             for (size_t j = 0; j < e.count; j += 5) {
                 int32_t weight = e.data.i32[j + 4];
                 if (weight == 0) {
                     continue;
                 }
                 res = mapCorrectedToRaw(e.data.i32 + j, 2, mapperInfo, /*clamp*/true);
                 if (res != OK) return res;
             }
         }
         for (auto rect : kRectsToCorrect) {
             e = request->find(rect);
             res = mapCorrectedRectToRaw(e.data.i32, e.count / 4, mapperInfo, /*clamp*/true);
             if (res != OK) return res;
         }
     }
     return OK;
 }

 status_t DistortionMapper::correctCaptureResult(CameraMetadata *result) {
     std::lock_guard<std::mutex> lock(mMutex);

     bool maxResolution = doesSettingsHaveMaxResolution(result);
     DistortionMapperInfo *mapperInfo = maxResolution ? &mDistortionMapperInfoMaximumResolution :
             &mDistortionMapperInfo;
     status_t res;

     if (!mapperInfo->mValidMapping) return OK;

     res = updateCalibration(*result, /*isStatic*/ false, maxResolution);
     if (res != OK) {
         ALOGE("Failure to update lens calibration information");
         return INVALID_OPERATION;
     }

     camera_metadata_entry_t e;
     e = result->find(ANDROID_DISTORTION_CORRECTION_MODE);
     if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
         for (auto region : kMeteringRegionsToCorrect) {
             e = result->find(region);
             for (size_t j = 0; j < e.count; j += 5) {
                 int32_t weight = e.data.i32[j + 4];
                 if (weight == 0) {
                     continue;
                 }
                 res = mapRawToCorrected(e.data.i32 + j, 2, mapperInfo, /*clamp*/true);
                 if (res != OK) return res;
             }
         }
         for (auto rect : kRectsToCorrect) {
             e = result->find(rect);
             res = mapRawRectToCorrected(e.data.i32, e.count / 4, mapperInfo, /*clamp*/true);
             if (res != OK) return res;
         }
         for (auto pts : kResultPointsToCorrectNoClamp) {
             e = result->find(pts);
             res = mapRawToCorrected(e.data.i32, e.count / 2, mapperInfo, /*clamp*/false);
             if (res != OK) return res;
         }
     }

     return OK;
 }

 // Utility methods; not guarded by mutex

 status_t DistortionMapper::updateCalibration(const CameraMetadata &result, bool isStatic,
         bool maxResolution) {
     camera_metadata_ro_entry_t calib, distortion;
     DistortionMapperInfo *mapperInfo =
             maxResolution ? &mDistortionMapperInfoMaximumResolution : &mDistortionMapperInfo;
     // We only need maximum resolution version of LENS_INTRINSIC_CALIBRATION and
     // LENS_DISTORTION since CaptureResults would still use the same key
     // regardless of sensor pixel mode.
     int calibrationKey =
         SessionConfigurationUtils::getAppropriateModeTag(ANDROID_LENS_INTRINSIC_CALIBRATION,
                 maxResolution && isStatic);
     int distortionKey =
         SessionConfigurationUtils::getAppropriateModeTag(ANDROID_LENS_DISTORTION,
                 maxResolution && isStatic);

     calib = result.find(calibrationKey);
     distortion = result.find(distortionKey);

     if (calib.count != 5) return BAD_VALUE;
     if (distortion.count != 5) return BAD_VALUE;

     // Skip redoing work if no change to calibration fields
     if (mapperInfo->mValidMapping &&
             mapperInfo->mFx == calib.data.f[0] &&
             mapperInfo->mFy == calib.data.f[1] &&
             mapperInfo->mCx == calib.data.f[2] &&
             mapperInfo->mCy == calib.data.f[3] &&
             mapperInfo->mS == calib.data.f[4]) {
         bool noChange = true;
         for (size_t i = 0; i < distortion.count; i++) {
             if (mapperInfo->mK[i] != distortion.data.f[i]) {
                 noChange = false;
                 break;
             }
         }
         if (noChange) return OK;
     }

     mapperInfo->mFx = calib.data.f[0];
     mapperInfo->mFy = calib.data.f[1];
     mapperInfo->mCx = calib.data.f[2];
     mapperInfo->mCy = calib.data.f[3];
     mapperInfo->mS = calib.data.f[4];

     mapperInfo->mInvFx = 1 / mapperInfo->mFx;
     mapperInfo->mInvFy = 1 / mapperInfo->mFy;

     for (size_t i = 0; i < distortion.count; i++) {
         mapperInfo->mK[i] = distortion.data.f[i];
     }

     mapperInfo->mValidMapping = true;
     // Need to recalculate grid
     mapperInfo->mValidGrids = false;

     return OK;
 }

 status_t DistortionMapper::mapRawToCorrected(int32_t *coordPairs, int coordCount,
         DistortionMapperInfo *mapperInfo, bool clamp, bool simple) {
     if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

     if (simple) return mapRawToCorrectedSimple(coordPairs, coordCount, mapperInfo, clamp);

     if (!mapperInfo->mValidGrids) {
         status_t res = buildGrids(mapperInfo);
         if (res != OK) return res;
     }

     for (int i = 0; i < coordCount * 2; i += 2) {
         const GridQuad *quad = findEnclosingQuad(coordPairs + i, mapperInfo->mDistortedGrid);
         if (quad == nullptr) {
             ALOGE("Raw to corrected mapping failure: No quad found for (%d, %d)",
                     *(coordPairs + i), *(coordPairs + i + 1));
             return INVALID_OPERATION;
         }
         ALOGV("src xy: %d, %d, enclosing quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
                 coordPairs[i], coordPairs[i+1],
                 quad->coords[0], quad->coords[1],
                 quad->coords[2], quad->coords[3],
                 quad->coords[4], quad->coords[5],
                 quad->coords[6], quad->coords[7]);

         const GridQuad *corrQuad = quad->src;
         if (corrQuad == nullptr) {
             ALOGE("Raw to corrected mapping failure: No src quad found");
             return INVALID_OPERATION;
         }
         ALOGV("              corr quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
                 corrQuad->coords[0], corrQuad->coords[1],
                 corrQuad->coords[2], corrQuad->coords[3],
                 corrQuad->coords[4], corrQuad->coords[5],
                 corrQuad->coords[6], corrQuad->coords[7]);

         float u = calculateUorV(coordPairs + i, *quad, /*calculateU*/ true);
         float v = calculateUorV(coordPairs + i, *quad, /*calculateU*/ false);

         ALOGV("uv: %f, %f", u, v);

         // Interpolate along top edge of corrected quad (which are axis-aligned) for x
         float corrX = corrQuad->coords[0] + u * (corrQuad->coords[2] - corrQuad->coords[0]);
         // Interpolate along left edge of corrected quad (which are axis-aligned) for y
         float corrY = corrQuad->coords[1] + v * (corrQuad->coords[7] - corrQuad->coords[1]);

         // Clamp to within active array
         if (clamp) {
             corrX = std::min(mapperInfo->mActiveWidth - 1, std::max(0.f, corrX));
             corrY = std::min(mapperInfo->mActiveHeight - 1, std::max(0.f, corrY));
         }

         coordPairs[i] = static_cast<int32_t>(std::round(corrX));
         coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY));
     }

     return OK;
 }

 status_t DistortionMapper::mapRawToCorrectedSimple(int32_t *coordPairs, int coordCount,
        const DistortionMapperInfo *mapperInfo, bool clamp) const {
     if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

     float scaleX = mapperInfo->mActiveWidth / mapperInfo->mArrayWidth;
     float scaleY = mapperInfo->mActiveHeight / mapperInfo->mArrayHeight;
     for (int i = 0; i < coordCount * 2; i += 2) {
         float x = coordPairs[i];
         float y = coordPairs[i + 1];
         float corrX = x * scaleX;
         float corrY = y * scaleY;
         if (clamp) {
             corrX = std::min(mapperInfo->mActiveWidth - 1, std::max(0.f, corrX));
             corrY = std::min(mapperInfo->mActiveHeight - 1, std::max(0.f, corrY));
         }
         coordPairs[i] = static_cast<int32_t>(std::round(corrX));
         coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY));
     }

     return OK;
 }

 status_t DistortionMapper::mapRawRectToCorrected(int32_t *rects, int rectCount,
        DistortionMapperInfo *mapperInfo, bool clamp, bool simple) {
     if (!mapperInfo->mValidMapping) return INVALID_OPERATION;
     for (int i = 0; i < rectCount * 4; i += 4) {
         // Map from (l, t, width, height) to (l, t, r, b)
         int32_t coords[4] = {
             rects[i],
             rects[i + 1],
             rects[i] + rects[i + 2] - 1,
             rects[i + 1] + rects[i + 3] - 1
         };

         mapRawToCorrected(coords, 2, mapperInfo, clamp, simple);

         // Map back to (l, t, width, height)
         rects[i] = coords[0];
         rects[i + 1] = coords[1];
         rects[i + 2] = coords[2] - coords[0] + 1;
         rects[i + 3] = coords[3] - coords[1] + 1;
     }

     return OK;
 }

 status_t DistortionMapper::mapCorrectedToRaw(int32_t *coordPairs, int coordCount,
        const DistortionMapperInfo *mapperInfo, bool clamp, bool simple) const {
     return mapCorrectedToRawImpl(coordPairs, coordCount, mapperInfo, clamp, simple);
 }

 template<typename T>
 status_t DistortionMapper::mapCorrectedToRawImpl(T *coordPairs, int coordCount,
        const DistortionMapperInfo *mapperInfo, bool clamp, bool simple) const {
     if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

     if (simple) return mapCorrectedToRawImplSimple(coordPairs, coordCount, mapperInfo, clamp);

     float activeCx = mapperInfo->mCx - mapperInfo->mArrayDiffX;
     float activeCy = mapperInfo->mCy - mapperInfo->mArrayDiffY;
     for (int i = 0; i < coordCount * 2; i += 2) {
         // Move to normalized space from active array space
         float ywi = (coordPairs[i + 1] - activeCy) * mapperInfo->mInvFy;
         float xwi = (coordPairs[i] - activeCx - mapperInfo->mS * ywi) * mapperInfo->mInvFx;
         // Apply distortion model to calculate raw image coordinates
         const std::array<float, 5> &kK = mapperInfo->mK;
         float rSq = xwi * xwi + ywi * ywi;
         float Fr = 1.f + (kK[0] * rSq) + (kK[1] * rSq * rSq) + (kK[2] * rSq * rSq * rSq);
         float xc = xwi * Fr + (kK[3] * 2 * xwi * ywi) + kK[4] * (rSq + 2 * xwi * xwi);
         float yc = ywi * Fr + (kK[4] * 2 * xwi * ywi) + kK[3] * (rSq + 2 * ywi * ywi);
         // Move back to image space
         float xr = mapperInfo->mFx * xc + mapperInfo->mS * yc + mapperInfo->mCx;
         float yr = mapperInfo->mFy * yc + mapperInfo->mCy;
         // Clamp to within pre-correction active array
         if (clamp) {
             xr = std::min(mapperInfo->mArrayWidth - 1, std::max(0.f, xr));
             yr = std::min(mapperInfo->mArrayHeight - 1, std::max(0.f, yr));
         }

         coordPairs[i] = static_cast<T>(std::round(xr));
         coordPairs[i + 1] = static_cast<T>(std::round(yr));
     }
     return OK;
 }

 template<typename T>
 status_t DistortionMapper::mapCorrectedToRawImplSimple(T *coordPairs, int coordCount,
        const DistortionMapperInfo *mapperInfo, bool clamp) const {
     if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

     float scaleX = mapperInfo->mArrayWidth / mapperInfo->mActiveWidth;
     float scaleY = mapperInfo->mArrayHeight / mapperInfo->mActiveHeight;
     for (int i = 0; i < coordCount * 2; i += 2) {
         float x = coordPairs[i];
         float y = coordPairs[i + 1];
         float rawX = x * scaleX;
         float rawY = y * scaleY;
         if (clamp) {
             rawX = std::min(mapperInfo->mArrayWidth - 1, std::max(0.f, rawX));
             rawY = std::min(mapperInfo->mArrayHeight - 1, std::max(0.f, rawY));
         }
         coordPairs[i] = static_cast<T>(std::round(rawX));
         coordPairs[i + 1] = static_cast<T>(std::round(rawY));
     }

     return OK;
 }

 status_t DistortionMapper::mapCorrectedRectToRaw(int32_t *rects, int rectCount,
        const DistortionMapperInfo *mapperInfo, bool clamp, bool simple) const {
     if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

     for (int i = 0; i < rectCount * 4; i += 4) {
         // Map from (l, t, width, height) to (l, t, r, b)
         int32_t coords[4] = {
             rects[i],
             rects[i + 1],
             rects[i] + rects[i + 2] - 1,
             rects[i + 1] + rects[i + 3] - 1
         };

         mapCorrectedToRaw(coords, 2, mapperInfo, clamp, simple);

         // Map back to (l, t, width, height)
         rects[i] = coords[0];
         rects[i + 1] = coords[1];
         rects[i + 2] = coords[2] - coords[0] + 1;
         rects[i + 3] = coords[3] - coords[1] + 1;
     }

     return OK;
 }

 status_t DistortionMapper::buildGrids(DistortionMapperInfo *mapperInfo) {
     if (mapperInfo->mCorrectedGrid.size() != kGridSize * kGridSize) {
         mapperInfo->mCorrectedGrid.resize(kGridSize * kGridSize);
         mapperInfo->mDistortedGrid.resize(kGridSize * kGridSize);
     }

     float gridMargin = mapperInfo->mArrayWidth * kGridMargin;
     float gridSpacingX = (mapperInfo->mArrayWidth + 2 * gridMargin) / kGridSize;
     float gridSpacingY = (mapperInfo->mArrayHeight + 2 * gridMargin) / kGridSize;

     size_t index = 0;
     float x = -gridMargin;
     for (size_t i = 0; i < kGridSize; i++, x += gridSpacingX) {
         float y = -gridMargin;
         for (size_t j = 0; j < kGridSize; j++, y += gridSpacingY, index++) {
             mapperInfo->mCorrectedGrid[index].src = nullptr;
             mapperInfo->mCorrectedGrid[index].coords = {
                 x, y,
                 x + gridSpacingX, y,
                 x + gridSpacingX, y + gridSpacingY,
                 x, y + gridSpacingY
             };
             mapperInfo->mDistortedGrid[index].src = &(mapperInfo->mCorrectedGrid[index]);
             mapperInfo->mDistortedGrid[index].coords = mapperInfo->mCorrectedGrid[index].coords;
             status_t res = mapCorrectedToRawImpl(mapperInfo->mDistortedGrid[index].coords.data(), 4,
                     mapperInfo, /*clamp*/false, /*simple*/false);
             if (res != OK) return res;
         }
     }

     mapperInfo->mValidGrids = true;
     return OK;
 }

 const DistortionMapper::GridQuad* DistortionMapper::findEnclosingQuad(
         const int32_t pt[2], const std::vector<GridQuad>& grid) {
     const float x = pt[0];
     const float y = pt[1];

     for (const GridQuad& quad : grid) {
         const float &x1 = quad.coords[0];
         const float &y1 = quad.coords[1];
         const float &x2 = quad.coords[2];
         const float &y2 = quad.coords[3];
         const float &x3 = quad.coords[4];
         const float &y3 = quad.coords[5];
         const float &x4 = quad.coords[6];
         const float &y4 = quad.coords[7];

         // Point-in-quad test:

         // Quad has corners P1-P4; if P is within the quad, then it is on the same side of all the
         // edges (or on top of one of the edges or corners), traversed in a consistent direction.
         // This means that the cross product of edge En = Pn->P(n+1 mod 4) and line Ep = Pn->P must
         // have the same sign (or be zero) for all edges.
         // For clockwise traversal, the sign should be negative or zero for Ep x En, indicating that
         // En is to the left of Ep, or overlapping.
         float s1 = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1);
         if (s1 > 0) continue;
         float s2 = (x - x2) * (y3 - y2) - (y - y2) * (x3 - x2);
         if (s2 > 0) continue;
         float s3 = (x - x3) * (y4 - y3) - (y - y3) * (x4 - x3);
         if (s3 > 0) continue;
         float s4 = (x - x4) * (y1 - y4) - (y - y4) * (x1 - x4);
         if (s4 > 0) continue;

         return &quad;
     }
     return nullptr;
 }

 float DistortionMapper::calculateUorV(const int32_t pt[2], const GridQuad& quad, bool calculateU) {
     const float x = pt[0];
     const float y = pt[1];
     const float &x1 = quad.coords[0];
     const float &y1 = quad.coords[1];
     const float &x2 = calculateU ? quad.coords[2] : quad.coords[6];
     const float &y2 = calculateU ? quad.coords[3] : quad.coords[7];
     const float &x3 = quad.coords[4];
     const float &y3 = quad.coords[5];
     const float &x4 = calculateU ? quad.coords[6] : quad.coords[2];
     const float &y4 = calculateU ? quad.coords[7] : quad.coords[3];

     float a = (x1 - x2) * (y1 - y2 + y3 - y4) - (y1 - y2) * (x1 - x2 + x3 - x4);
     float b = (x - x1) * (y1 - y2 + y3 - y4) + (x1 - x2) * (y4 - y1) -
               (y - y1) * (x1 - x2 + x3 - x4) - (y1 - y2) * (x4 - x1);
     float c = (x - x1) * (y4 - y1) - (y - y1) * (x4 - x1);

     if (a == 0) {
         // One solution may happen if edges are parallel
         float u0 = -c / b;
         ALOGV("u0: %.9g, b: %f, c: %f", u0, b, c);
         return u0;
     }

     float det = b * b - 4 * a * c;
     if (det < 0) {
         // Validation check - should not happen if pt is within the quad
         ALOGE("Bad determinant! a: %f, b: %f, c: %f, det: %f", a,b,c,det);
         return -1;
     }

     // Select more numerically stable solution
     float sqdet = b > 0 ? -std::sqrt(det) : std::sqrt(det);

     float u1 = (-b + sqdet) / (2 * a);
     ALOGV("u1: %.9g", u1);
     if (0 - kFloatFuzz < u1 && u1 < 1 + kFloatFuzz) return u1;

     float u2 = c / (a * u1);
     ALOGV("u2: %.9g", u2);
     if (0 - kFloatFuzz < u2 && u2 < 1 + kFloatFuzz) return u2;

     // Last resort, return the smaller-magnitude solution
     return fabs(u1) < fabs(u2) ? u1 : u2;
 }

 } // namespace camera3

 } // namespace android
	/*
	* Copyright (C) 2018 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	#define LOG_TAG "Camera3-DistMapper"
	#define ATRACE_TAG ATRACE_TAG_CAMERA
	//#define LOG_NDEBUG 0

	#include <algorithm>
	#include <cmath>

	#include "device3/DistortionMapper.h"
	#include "utils/SessionConfigurationUtilsHost.h"

	namespace android {

	namespace camera3 {


	DistortionMapper::DistortionMapper() {
	initRemappedKeys();
	}

	void DistortionMapper::initRemappedKeys() {
	mRemappedKeys.insert(
	kMeteringRegionsToCorrect.begin(),
	kMeteringRegionsToCorrect.end());
	mRemappedKeys.insert(
	kRectsToCorrect.begin(),
	kRectsToCorrect.end());
	mRemappedKeys.insert(
	kResultPointsToCorrectNoClamp.begin(),
	kResultPointsToCorrectNoClamp.end());
	mRemappedKeys.insert(ANDROID_DISTORTION_CORRECTION_MODE);
	}

	bool DistortionMapper::isDistortionSupported(const CameraMetadata &deviceInfo) {
	bool isDistortionCorrectionSupported = false;
	camera_metadata_ro_entry_t distortionCorrectionModes =
	deviceInfo.find(ANDROID_DISTORTION_CORRECTION_AVAILABLE_MODES);
	for (size_t i = 0; i < distortionCorrectionModes.count; i++) {
	if (distortionCorrectionModes.data.u8[i] !=
	ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
	isDistortionCorrectionSupported = true;
	break;
	}
	}
	return isDistortionCorrectionSupported;
	}

	status_t DistortionMapper::setupStaticInfo(const CameraMetadata &deviceInfo) {
	std::lock_guard<std::mutex> lock(mMutex);
	status_t res = setupStaticInfoLocked(deviceInfo, /maxResolution/false);
	if (res != OK) {
	return res;
	}

	bool mMaxResolution = SessionConfigurationUtils::supportsUltraHighResolutionCapture(deviceInfo);
	if (mMaxResolution) {
	res = setupStaticInfoLocked(deviceInfo, /maxResolution/true);
	}
	return res;
	}

	status_t DistortionMapper::setupStaticInfoLocked(const CameraMetadata &deviceInfo,
	bool maxResolution) {
	DistortionMapperInfo *mapperInfo = maxResolution ? &mDistortionMapperInfoMaximumResolution :
	&mDistortionMapperInfo;

	camera_metadata_ro_entry_t array;

	array = deviceInfo.find(
	SessionConfigurationUtils::getAppropriateModeTag(
	ANDROID_SENSOR_INFO_PRE_CORRECTION_ACTIVE_ARRAY_SIZE, maxResolution));
	if (array.count != 4) return BAD_VALUE;

	float arrayX = static_cast<float>(array.data.i32[0]);
	float arrayY = static_cast<float>(array.data.i32[1]);
	mapperInfo->mArrayWidth = static_cast<float>(array.data.i32[2]);
	mapperInfo->mArrayHeight = static_cast<float>(array.data.i32[3]);

	array = deviceInfo.find(
	SessionConfigurationUtils::getAppropriateModeTag(
	ANDROID_SENSOR_INFO_ACTIVE_ARRAY_SIZE, maxResolution));
	if (array.count != 4) return BAD_VALUE;

	float activeX = static_cast<float>(array.data.i32[0]);
	float activeY = static_cast<float>(array.data.i32[1]);
	mapperInfo->mActiveWidth = static_cast<float>(array.data.i32[2]);
	mapperInfo->mActiveHeight = static_cast<float>(array.data.i32[3]);

	mapperInfo->mArrayDiffX = activeX - arrayX;
	mapperInfo->mArrayDiffY = activeY - arrayY;

	return updateCalibration(deviceInfo, /isStatic/ true, maxResolution);
	}

	static bool doesSettingsHaveMaxResolution(const CameraMetadata *settings) {
	if (settings == nullptr) {
	return false;
	}
	// First we get the sensorPixelMode from the settings metadata.
	camera_metadata_ro_entry sensorPixelModeEntry = settings->find(ANDROID_SENSOR_PIXEL_MODE);
	if (sensorPixelModeEntry.count != 0) {
	return (sensorPixelModeEntry.data.u8[0] == ANDROID_SENSOR_PIXEL_MODE_MAXIMUM_RESOLUTION);
	}
	return false;
	}

	bool DistortionMapper::calibrationValid() const {
	std::lock_guard<std::mutex> lock(mMutex);
	bool isValid = mDistortionMapperInfo.mValidMapping;
	if (mMaxResolution) {
	isValid = isValid && mDistortionMapperInfoMaximumResolution.mValidMapping;
	}
	return isValid;
	}

	status_t DistortionMapper::correctCaptureRequest(CameraMetadata *request) {
	std::lock_guard<std::mutex> lock(mMutex);
	status_t res;

	bool maxResolution = doesSettingsHaveMaxResolution(request);
	DistortionMapperInfo *mapperInfo = maxResolution ? &mDistortionMapperInfoMaximumResolution :
	&mDistortionMapperInfo;

	if (!mapperInfo->mValidMapping) return OK;

	camera_metadata_entry_t e;
	e = request->find(ANDROID_DISTORTION_CORRECTION_MODE);
	if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
	for (auto region : kMeteringRegionsToCorrect) {
	e = request->find(region);
	for (size_t j = 0; j < e.count; j += 5) {
	int32_t weight = e.data.i32[j + 4];
	if (weight == 0) {
	continue;
	}
	res = mapCorrectedToRaw(e.data.i32 + j, 2, mapperInfo, /clamp/true);
	if (res != OK) return res;
	}
	}
	for (auto rect : kRectsToCorrect) {
	e = request->find(rect);
	res = mapCorrectedRectToRaw(e.data.i32, e.count / 4, mapperInfo, /clamp/true);
	if (res != OK) return res;
	}
	}
	return OK;
	}

	status_t DistortionMapper::correctCaptureResult(CameraMetadata *result) {
	std::lock_guard<std::mutex> lock(mMutex);

	bool maxResolution = doesSettingsHaveMaxResolution(result);
	DistortionMapperInfo *mapperInfo = maxResolution ? &mDistortionMapperInfoMaximumResolution :
	&mDistortionMapperInfo;
	status_t res;

	if (!mapperInfo->mValidMapping) return OK;

	res = updateCalibration(result, /isStatic*/ false, maxResolution);
	if (res != OK) {
	ALOGE("Failure to update lens calibration information");
	return INVALID_OPERATION;
	}

	camera_metadata_entry_t e;
	e = result->find(ANDROID_DISTORTION_CORRECTION_MODE);
	if (e.count != 0 && e.data.u8[0] != ANDROID_DISTORTION_CORRECTION_MODE_OFF) {
	for (auto region : kMeteringRegionsToCorrect) {
	e = result->find(region);
	for (size_t j = 0; j < e.count; j += 5) {
	int32_t weight = e.data.i32[j + 4];
	if (weight == 0) {
	continue;
	}
	res = mapRawToCorrected(e.data.i32 + j, 2, mapperInfo, /clamp/true);
	if (res != OK) return res;
	}
	}
	for (auto rect : kRectsToCorrect) {
	e = result->find(rect);
	res = mapRawRectToCorrected(e.data.i32, e.count / 4, mapperInfo, /clamp/true);
	if (res != OK) return res;
	}
	for (auto pts : kResultPointsToCorrectNoClamp) {
	e = result->find(pts);
	res = mapRawToCorrected(e.data.i32, e.count / 2, mapperInfo, /clamp/false);
	if (res != OK) return res;
	}
	}

	return OK;
	}

	// Utility methods; not guarded by mutex

	status_t DistortionMapper::updateCalibration(const CameraMetadata &result, bool isStatic,
	bool maxResolution) {
	camera_metadata_ro_entry_t calib, distortion;
	DistortionMapperInfo *mapperInfo =
	maxResolution ? &mDistortionMapperInfoMaximumResolution : &mDistortionMapperInfo;
	// We only need maximum resolution version of LENS_INTRINSIC_CALIBRATION and
	// LENS_DISTORTION since CaptureResults would still use the same key
	// regardless of sensor pixel mode.
	int calibrationKey =
	SessionConfigurationUtils::getAppropriateModeTag(ANDROID_LENS_INTRINSIC_CALIBRATION,
	maxResolution && isStatic);
	int distortionKey =
	SessionConfigurationUtils::getAppropriateModeTag(ANDROID_LENS_DISTORTION,
	maxResolution && isStatic);

	calib = result.find(calibrationKey);
	distortion = result.find(distortionKey);

	if (calib.count != 5) return BAD_VALUE;
	if (distortion.count != 5) return BAD_VALUE;

	// Skip redoing work if no change to calibration fields
	if (mapperInfo->mValidMapping &&
	mapperInfo->mFx == calib.data.f[0] &&
	mapperInfo->mFy == calib.data.f[1] &&
	mapperInfo->mCx == calib.data.f[2] &&
	mapperInfo->mCy == calib.data.f[3] &&
	mapperInfo->mS == calib.data.f[4]) {
	bool noChange = true;
	for (size_t i = 0; i < distortion.count; i++) {
	if (mapperInfo->mK[i] != distortion.data.f[i]) {
	noChange = false;
	break;
	}
	}
	if (noChange) return OK;
	}

	mapperInfo->mFx = calib.data.f[0];
	mapperInfo->mFy = calib.data.f[1];
	mapperInfo->mCx = calib.data.f[2];
	mapperInfo->mCy = calib.data.f[3];
	mapperInfo->mS = calib.data.f[4];

	mapperInfo->mInvFx = 1 / mapperInfo->mFx;
	mapperInfo->mInvFy = 1 / mapperInfo->mFy;

	for (size_t i = 0; i < distortion.count; i++) {
	mapperInfo->mK[i] = distortion.data.f[i];
	}

	mapperInfo->mValidMapping = true;
	// Need to recalculate grid
	mapperInfo->mValidGrids = false;

	return OK;
	}

	status_t DistortionMapper::mapRawToCorrected(int32_t *coordPairs, int coordCount,
	DistortionMapperInfo *mapperInfo, bool clamp, bool simple) {
	if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

	if (simple) return mapRawToCorrectedSimple(coordPairs, coordCount, mapperInfo, clamp);

	if (!mapperInfo->mValidGrids) {
	status_t res = buildGrids(mapperInfo);
	if (res != OK) return res;
	}

	for (int i = 0; i < coordCount * 2; i += 2) {
	const GridQuad *quad = findEnclosingQuad(coordPairs + i, mapperInfo->mDistortedGrid);
	if (quad == nullptr) {
	ALOGE("Raw to corrected mapping failure: No quad found for (%d, %d)",
	(coordPairs + i), (coordPairs + i + 1));
	return INVALID_OPERATION;
	}
	ALOGV("src xy: %d, %d, enclosing quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
	coordPairs[i], coordPairs[i+1],
	quad->coords[0], quad->coords[1],
	quad->coords[2], quad->coords[3],
	quad->coords[4], quad->coords[5],
	quad->coords[6], quad->coords[7]);

	const GridQuad *corrQuad = quad->src;
	if (corrQuad == nullptr) {
	ALOGE("Raw to corrected mapping failure: No src quad found");
	return INVALID_OPERATION;
	}
	ALOGV(" corr quad: (%f, %f), (%f, %f), (%f, %f), (%f, %f)",
	corrQuad->coords[0], corrQuad->coords[1],
	corrQuad->coords[2], corrQuad->coords[3],
	corrQuad->coords[4], corrQuad->coords[5],
	corrQuad->coords[6], corrQuad->coords[7]);

	float u = calculateUorV(coordPairs + i, quad, /calculateU*/ true);
	float v = calculateUorV(coordPairs + i, quad, /calculateU*/ false);

	ALOGV("uv: %f, %f", u, v);

	// Interpolate along top edge of corrected quad (which are axis-aligned) for x
	float corrX = corrQuad->coords[0] + u * (corrQuad->coords[2] - corrQuad->coords[0]);
	// Interpolate along left edge of corrected quad (which are axis-aligned) for y
	float corrY = corrQuad->coords[1] + v * (corrQuad->coords[7] - corrQuad->coords[1]);

	// Clamp to within active array
	if (clamp) {
	corrX = std::min(mapperInfo->mActiveWidth - 1, std::max(0.f, corrX));
	corrY = std::min(mapperInfo->mActiveHeight - 1, std::max(0.f, corrY));
	}

	coordPairs[i] = static_cast<int32_t>(std::round(corrX));
	coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY));
	}

	return OK;
	}

	status_t DistortionMapper::mapRawToCorrectedSimple(int32_t *coordPairs, int coordCount,
	const DistortionMapperInfo *mapperInfo, bool clamp) const {
	if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

	float scaleX = mapperInfo->mActiveWidth / mapperInfo->mArrayWidth;
	float scaleY = mapperInfo->mActiveHeight / mapperInfo->mArrayHeight;
	for (int i = 0; i < coordCount * 2; i += 2) {
	float x = coordPairs[i];
	float y = coordPairs[i + 1];
	float corrX = x * scaleX;
	float corrY = y * scaleY;
	if (clamp) {
	corrX = std::min(mapperInfo->mActiveWidth - 1, std::max(0.f, corrX));
	corrY = std::min(mapperInfo->mActiveHeight - 1, std::max(0.f, corrY));
	}
	coordPairs[i] = static_cast<int32_t>(std::round(corrX));
	coordPairs[i + 1] = static_cast<int32_t>(std::round(corrY));
	}

	return OK;
	}

	status_t DistortionMapper::mapRawRectToCorrected(int32_t *rects, int rectCount,
	DistortionMapperInfo *mapperInfo, bool clamp, bool simple) {
	if (!mapperInfo->mValidMapping) return INVALID_OPERATION;
	for (int i = 0; i < rectCount * 4; i += 4) {
	// Map from (l, t, width, height) to (l, t, r, b)
	int32_t coords[4] = {
	rects[i],
	rects[i + 1],
	rects[i] + rects[i + 2] - 1,
	rects[i + 1] + rects[i + 3] - 1
	};

	mapRawToCorrected(coords, 2, mapperInfo, clamp, simple);

	// Map back to (l, t, width, height)
	rects[i] = coords[0];
	rects[i + 1] = coords[1];
	rects[i + 2] = coords[2] - coords[0] + 1;
	rects[i + 3] = coords[3] - coords[1] + 1;
	}

	return OK;
	}

	status_t DistortionMapper::mapCorrectedToRaw(int32_t *coordPairs, int coordCount,
	const DistortionMapperInfo *mapperInfo, bool clamp, bool simple) const {
	return mapCorrectedToRawImpl(coordPairs, coordCount, mapperInfo, clamp, simple);
	}

	template<typename T>
	status_t DistortionMapper::mapCorrectedToRawImpl(T *coordPairs, int coordCount,
	const DistortionMapperInfo *mapperInfo, bool clamp, bool simple) const {
	if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

	if (simple) return mapCorrectedToRawImplSimple(coordPairs, coordCount, mapperInfo, clamp);

	float activeCx = mapperInfo->mCx - mapperInfo->mArrayDiffX;
	float activeCy = mapperInfo->mCy - mapperInfo->mArrayDiffY;
	for (int i = 0; i < coordCount * 2; i += 2) {
	// Move to normalized space from active array space
	float ywi = (coordPairs[i + 1] - activeCy) * mapperInfo->mInvFy;
	float xwi = (coordPairs[i] - activeCx - mapperInfo->mS * ywi) * mapperInfo->mInvFx;
	// Apply distortion model to calculate raw image coordinates
	const std::array<float, 5> &kK = mapperInfo->mK;
	float rSq = xwi * xwi + ywi * ywi;
	float Fr = 1.f + (kK[0] * rSq) + (kK[1] * rSq * rSq) + (kK[2] * rSq * rSq * rSq);
	float xc = xwi * Fr + (kK[3] * 2 * xwi * ywi) + kK[4] * (rSq + 2 * xwi * xwi);
	float yc = ywi * Fr + (kK[4] * 2 * xwi * ywi) + kK[3] * (rSq + 2 * ywi * ywi);
	// Move back to image space
	float xr = mapperInfo->mFx * xc + mapperInfo->mS * yc + mapperInfo->mCx;
	float yr = mapperInfo->mFy * yc + mapperInfo->mCy;
	// Clamp to within pre-correction active array
	if (clamp) {
	xr = std::min(mapperInfo->mArrayWidth - 1, std::max(0.f, xr));
	yr = std::min(mapperInfo->mArrayHeight - 1, std::max(0.f, yr));
	}

	coordPairs[i] = static_cast<T>(std::round(xr));
	coordPairs[i + 1] = static_cast<T>(std::round(yr));
	}
	return OK;
	}

	template<typename T>
	status_t DistortionMapper::mapCorrectedToRawImplSimple(T *coordPairs, int coordCount,
	const DistortionMapperInfo *mapperInfo, bool clamp) const {
	if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

	float scaleX = mapperInfo->mArrayWidth / mapperInfo->mActiveWidth;
	float scaleY = mapperInfo->mArrayHeight / mapperInfo->mActiveHeight;
	for (int i = 0; i < coordCount * 2; i += 2) {
	float x = coordPairs[i];
	float y = coordPairs[i + 1];
	float rawX = x * scaleX;
	float rawY = y * scaleY;
	if (clamp) {
	rawX = std::min(mapperInfo->mArrayWidth - 1, std::max(0.f, rawX));
	rawY = std::min(mapperInfo->mArrayHeight - 1, std::max(0.f, rawY));
	}
	coordPairs[i] = static_cast<T>(std::round(rawX));
	coordPairs[i + 1] = static_cast<T>(std::round(rawY));
	}

	return OK;
	}

	status_t DistortionMapper::mapCorrectedRectToRaw(int32_t *rects, int rectCount,
	const DistortionMapperInfo *mapperInfo, bool clamp, bool simple) const {
	if (!mapperInfo->mValidMapping) return INVALID_OPERATION;

	for (int i = 0; i < rectCount * 4; i += 4) {
	// Map from (l, t, width, height) to (l, t, r, b)
	int32_t coords[4] = {
	rects[i],
	rects[i + 1],
	rects[i] + rects[i + 2] - 1,
	rects[i + 1] + rects[i + 3] - 1
	};

	mapCorrectedToRaw(coords, 2, mapperInfo, clamp, simple);

	// Map back to (l, t, width, height)
	rects[i] = coords[0];
	rects[i + 1] = coords[1];
	rects[i + 2] = coords[2] - coords[0] + 1;
	rects[i + 3] = coords[3] - coords[1] + 1;
	}

	return OK;
	}

	status_t DistortionMapper::buildGrids(DistortionMapperInfo *mapperInfo) {
	if (mapperInfo->mCorrectedGrid.size() != kGridSize * kGridSize) {
	mapperInfo->mCorrectedGrid.resize(kGridSize * kGridSize);
	mapperInfo->mDistortedGrid.resize(kGridSize * kGridSize);
	}

	float gridMargin = mapperInfo->mArrayWidth * kGridMargin;
	float gridSpacingX = (mapperInfo->mArrayWidth + 2 * gridMargin) / kGridSize;
	float gridSpacingY = (mapperInfo->mArrayHeight + 2 * gridMargin) / kGridSize;

	size_t index = 0;
	float x = -gridMargin;
	for (size_t i = 0; i < kGridSize; i++, x += gridSpacingX) {
	float y = -gridMargin;
	for (size_t j = 0; j < kGridSize; j++, y += gridSpacingY, index++) {
	mapperInfo->mCorrectedGrid[index].src = nullptr;
	mapperInfo->mCorrectedGrid[index].coords = {
	x, y,
	x + gridSpacingX, y,
	x + gridSpacingX, y + gridSpacingY,
	x, y + gridSpacingY
	};
	mapperInfo->mDistortedGrid[index].src = &(mapperInfo->mCorrectedGrid[index]);
	mapperInfo->mDistortedGrid[index].coords = mapperInfo->mCorrectedGrid[index].coords;
	status_t res = mapCorrectedToRawImpl(mapperInfo->mDistortedGrid[index].coords.data(), 4,
	mapperInfo, /clamp/false, /simple/false);
	if (res != OK) return res;
	}
	}

	mapperInfo->mValidGrids = true;
	return OK;
	}

	const DistortionMapper::GridQuad* DistortionMapper::findEnclosingQuad(
	const int32_t pt[2], const std::vector<GridQuad>& grid) {
	const float x = pt[0];
	const float y = pt[1];

	for (const GridQuad& quad : grid) {
	const float &x1 = quad.coords[0];
	const float &y1 = quad.coords[1];
	const float &x2 = quad.coords[2];
	const float &y2 = quad.coords[3];
	const float &x3 = quad.coords[4];
	const float &y3 = quad.coords[5];
	const float &x4 = quad.coords[6];
	const float &y4 = quad.coords[7];

	// Point-in-quad test:

	// Quad has corners P1-P4; if P is within the quad, then it is on the same side of all the
	// edges (or on top of one of the edges or corners), traversed in a consistent direction.
	// This means that the cross product of edge En = Pn->P(n+1 mod 4) and line Ep = Pn->P must
	// have the same sign (or be zero) for all edges.
	// For clockwise traversal, the sign should be negative or zero for Ep x En, indicating that
	// En is to the left of Ep, or overlapping.
	float s1 = (x - x1) * (y2 - y1) - (y - y1) * (x2 - x1);
	if (s1 > 0) continue;
	float s2 = (x - x2) * (y3 - y2) - (y - y2) * (x3 - x2);
	if (s2 > 0) continue;
	float s3 = (x - x3) * (y4 - y3) - (y - y3) * (x4 - x3);
	if (s3 > 0) continue;
	float s4 = (x - x4) * (y1 - y4) - (y - y4) * (x1 - x4);
	if (s4 > 0) continue;

	return &quad;
	}
	return nullptr;
	}

	float DistortionMapper::calculateUorV(const int32_t pt[2], const GridQuad& quad, bool calculateU) {
	const float x = pt[0];
	const float y = pt[1];
	const float &x1 = quad.coords[0];
	const float &y1 = quad.coords[1];
	const float &x2 = calculateU ? quad.coords[2] : quad.coords[6];
	const float &y2 = calculateU ? quad.coords[3] : quad.coords[7];
	const float &x3 = quad.coords[4];
	const float &y3 = quad.coords[5];
	const float &x4 = calculateU ? quad.coords[6] : quad.coords[2];
	const float &y4 = calculateU ? quad.coords[7] : quad.coords[3];

	float a = (x1 - x2) * (y1 - y2 + y3 - y4) - (y1 - y2) * (x1 - x2 + x3 - x4);
	float b = (x - x1) * (y1 - y2 + y3 - y4) + (x1 - x2) * (y4 - y1) -
	(y - y1) * (x1 - x2 + x3 - x4) - (y1 - y2) * (x4 - x1);
	float c = (x - x1) * (y4 - y1) - (y - y1) * (x4 - x1);

	if (a == 0) {
	// One solution may happen if edges are parallel
	float u0 = -c / b;
	ALOGV("u0: %.9g, b: %f, c: %f", u0, b, c);
	return u0;
	}

	float det = b * b - 4 * a * c;
	if (det < 0) {
	// Validation check - should not happen if pt is within the quad
	ALOGE("Bad determinant! a: %f, b: %f, c: %f, det: %f", a,b,c,det);
	return -1;
	}

	// Select more numerically stable solution
	float sqdet = b > 0 ? -std::sqrt(det) : std::sqrt(det);

	float u1 = (-b + sqdet) / (2 * a);
	ALOGV("u1: %.9g", u1);
	if (0 - kFloatFuzz < u1 && u1 < 1 + kFloatFuzz) return u1;

	float u2 = c / (a * u1);
	ALOGV("u2: %.9g", u2);
	if (0 - kFloatFuzz < u2 && u2 < 1 + kFloatFuzz) return u2;

	// Last resort, return the smaller-magnitude solution
	return fabs(u1) < fabs(u2) ? u1 : u2;
	}

	} // namespace camera3

	} // namespace android