diff options
Diffstat (limited to 'jni/feature_mos/src/mosaic/Blend.cpp')
| -rw-r--r-- | jni/feature_mos/src/mosaic/Blend.cpp | 1410 |
1 files changed, 1410 insertions, 0 deletions
diff --git a/jni/feature_mos/src/mosaic/Blend.cpp b/jni/feature_mos/src/mosaic/Blend.cpp new file mode 100644 index 000000000..ef983ff67 --- /dev/null +++ b/jni/feature_mos/src/mosaic/Blend.cpp @@ -0,0 +1,1410 @@ +/* + * Copyright (C) 2011 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. + */ + +/////////////////////////////////////////////////// +// Blend.cpp +// $Id: Blend.cpp,v 1.22 2011/06/24 04:22:14 mbansal Exp $ + +#include <string.h> + +#include "Interp.h" +#include "Blend.h" + +#include "Geometry.h" +#include "trsMatrix.h" + +#include "Log.h" +#define LOG_TAG "BLEND" + +Blend::Blend() +{ + m_wb.blendingType = BLEND_TYPE_NONE; +} + +Blend::~Blend() +{ + if (m_pFrameVPyr) free(m_pFrameVPyr); + if (m_pFrameUPyr) free(m_pFrameUPyr); + if (m_pFrameYPyr) free(m_pFrameYPyr); +} + +int Blend::initialize(int blendingType, int stripType, int frame_width, int frame_height) +{ + this->width = frame_width; + this->height = frame_height; + this->m_wb.blendingType = blendingType; + this->m_wb.stripType = stripType; + + m_wb.blendRange = m_wb.blendRangeUV = BLEND_RANGE_DEFAULT; + m_wb.nlevs = m_wb.blendRange; + m_wb.nlevsC = m_wb.blendRangeUV; + + if (m_wb.nlevs <= 0) m_wb.nlevs = 1; // Need levels for YUV processing + if (m_wb.nlevsC > m_wb.nlevs) m_wb.nlevsC = m_wb.nlevs; + + m_wb.roundoffOverlap = 1.5; + + m_pFrameYPyr = NULL; + m_pFrameUPyr = NULL; + m_pFrameVPyr = NULL; + + m_pFrameYPyr = PyramidShort::allocatePyramidPacked(m_wb.nlevs, (unsigned short) width, (unsigned short) height, BORDER); + m_pFrameUPyr = PyramidShort::allocatePyramidPacked(m_wb.nlevsC, (unsigned short) (width), (unsigned short) (height), BORDER); + m_pFrameVPyr = PyramidShort::allocatePyramidPacked(m_wb.nlevsC, (unsigned short) (width), (unsigned short) (height), BORDER); + + if (!m_pFrameYPyr || !m_pFrameUPyr || !m_pFrameVPyr) + { + LOGE("Error: Could not allocate pyramids for blending"); + return BLEND_RET_ERROR_MEMORY; + } + + return BLEND_RET_OK; +} + +inline double max(double a, double b) { return a > b ? a : b; } +inline double min(double a, double b) { return a < b ? a : b; } + +void Blend::AlignToMiddleFrame(MosaicFrame **frames, int frames_size) +{ + // Unwarp this frame and Warp the others to match + MosaicFrame *mb = NULL; + MosaicFrame *ref = frames[int(frames_size/2)]; // Middle frame + + double invtrs[3][3]; + inv33d(ref->trs, invtrs); + + for(int mfit = 0; mfit < frames_size; mfit++) + { + mb = frames[mfit]; + double temp[3][3]; + mult33d(temp, invtrs, mb->trs); + memcpy(mb->trs, temp, sizeof(temp)); + normProjMat33d(mb->trs); + } +} + +int Blend::runBlend(MosaicFrame **oframes, MosaicFrame **rframes, + int frames_size, + ImageType &imageMosaicYVU, int &mosaicWidth, int &mosaicHeight, + float &progress, bool &cancelComputation) +{ + int ret; + int numCenters; + + MosaicFrame **frames; + + // For THIN strip mode, accept all frames for blending + if (m_wb.stripType == STRIP_TYPE_THIN) + { + frames = oframes; + } + else // For WIDE strip mode, first select the relevant frames to blend. + { + SelectRelevantFrames(oframes, frames_size, rframes, frames_size); + frames = rframes; + } + + ComputeBlendParameters(frames, frames_size, true); + numCenters = frames_size; + + if (numCenters == 0) + { + LOGE("Error: No frames to blend"); + return BLEND_RET_ERROR; + } + + if (!(m_AllSites = m_Triangulator.allocMemory(numCenters))) + { + return BLEND_RET_ERROR_MEMORY; + } + + // Bounding rectangle (real numbers) of the final mosaic computed by projecting + // each input frame into the mosaic coordinate system. + BlendRect global_rect; + + global_rect.lft = global_rect.bot = 2e30; // min values + global_rect.rgt = global_rect.top = -2e30; // max values + MosaicFrame *mb = NULL; + double halfwidth = width / 2.0; + double halfheight = height / 2.0; + + double z, x0, y0, x1, y1, x2, y2, x3, y3; + + // Corners of the left-most and right-most frames respectively in the + // mosaic coordinate system. + double xLeftCorners[2] = {2e30, 2e30}; + double xRightCorners[2] = {-2e30, -2e30}; + + // Corners of the top-most and bottom-most frames respectively in the + // mosaic coordinate system. + double yTopCorners[2] = {2e30, 2e30}; + double yBottomCorners[2] = {-2e30, -2e30}; + + + // Determine the extents of the final mosaic + CSite *csite = m_AllSites ; + for(int mfit = 0; mfit < frames_size; mfit++) + { + mb = frames[mfit]; + + // Compute clipping for this frame's rect + FrameToMosaicRect(mb->width, mb->height, mb->trs, mb->brect); + // Clip global rect using this frame's rect + ClipRect(mb->brect, global_rect); + + // Calculate the corner points + FrameToMosaic(mb->trs, 0.0, 0.0, x0, y0); + FrameToMosaic(mb->trs, 0.0, mb->height-1.0, x1, y1); + FrameToMosaic(mb->trs, mb->width-1.0, mb->height-1.0, x2, y2); + FrameToMosaic(mb->trs, mb->width-1.0, 0.0, x3, y3); + + if(x0 < xLeftCorners[0] || x1 < xLeftCorners[1]) // If either of the left corners is lower + { + xLeftCorners[0] = x0; + xLeftCorners[1] = x1; + } + + if(x3 > xRightCorners[0] || x2 > xRightCorners[1]) // If either of the right corners is higher + { + xRightCorners[0] = x3; + xRightCorners[1] = x2; + } + + if(y0 < yTopCorners[0] || y3 < yTopCorners[1]) // If either of the top corners is lower + { + yTopCorners[0] = y0; + yTopCorners[1] = y3; + } + + if(y1 > yBottomCorners[0] || y2 > yBottomCorners[1]) // If either of the bottom corners is higher + { + yBottomCorners[0] = y1; + yBottomCorners[1] = y2; + } + + + // Compute the centroid of the warped region + FindQuadCentroid(x0, y0, x1, y1, x2, y2, x3, y3, csite->getVCenter().x, csite->getVCenter().y); + + csite->setMb(mb); + csite++; + } + + // Get origin and sizes + + // Bounding rectangle (int numbers) of the final mosaic computed by projecting + // each input frame into the mosaic coordinate system. + MosaicRect fullRect; + + fullRect.left = (int) floor(global_rect.lft); // min-x + fullRect.top = (int) floor(global_rect.bot); // min-y + fullRect.right = (int) ceil(global_rect.rgt); // max-x + fullRect.bottom = (int) ceil(global_rect.top);// max-y + Mwidth = (unsigned short) (fullRect.right - fullRect.left + 1); + Mheight = (unsigned short) (fullRect.bottom - fullRect.top + 1); + + int xLeftMost, xRightMost; + int yTopMost, yBottomMost; + + // Rounding up, so that we don't include the gray border. + xLeftMost = max(0, max(xLeftCorners[0], xLeftCorners[1]) - fullRect.left + 1); + xRightMost = min(Mwidth - 1, min(xRightCorners[0], xRightCorners[1]) - fullRect.left - 1); + + yTopMost = max(0, max(yTopCorners[0], yTopCorners[1]) - fullRect.top + 1); + yBottomMost = min(Mheight - 1, min(yBottomCorners[0], yBottomCorners[1]) - fullRect.top - 1); + + if (xRightMost <= xLeftMost || yBottomMost <= yTopMost) + { + LOGE("RunBlend: aborting -consistency check failed," + "(xLeftMost, xRightMost, yTopMost, yBottomMost): (%d, %d, %d, %d)", + xLeftMost, xRightMost, yTopMost, yBottomMost); + return BLEND_RET_ERROR; + } + + // Make sure image width is multiple of 4 + Mwidth = (unsigned short) ((Mwidth + 3) & ~3); + Mheight = (unsigned short) ((Mheight + 3) & ~3); // Round up. + + ret = MosaicSizeCheck(LIMIT_SIZE_MULTIPLIER, LIMIT_HEIGHT_MULTIPLIER); + if (ret != BLEND_RET_OK) + { + LOGE("RunBlend: aborting - mosaic size check failed, " + "(frame_width, frame_height) vs (mosaic_width, mosaic_height): " + "(%d, %d) vs (%d, %d)", width, height, Mwidth, Mheight); + return ret; + } + + LOGI("Allocate mosaic image for blending - size: %d x %d", Mwidth, Mheight); + YUVinfo *imgMos = YUVinfo::allocateImage(Mwidth, Mheight); + if (imgMos == NULL) + { + LOGE("RunBlend: aborting - couldn't alloc %d x %d mosaic image", Mwidth, Mheight); + return BLEND_RET_ERROR_MEMORY; + } + + // Set the Y image to 255 so we can distinguish when frame idx are written to it + memset(imgMos->Y.ptr[0], 255, (imgMos->Y.width * imgMos->Y.height)); + // Set the v and u images to black + memset(imgMos->V.ptr[0], 128, (imgMos->V.width * imgMos->V.height) << 1); + + // Do the triangulation. It returns a sorted list of edges + SEdgeVector *edge; + int n = m_Triangulator.triangulate(&edge, numCenters, width, height); + m_Triangulator.linkNeighbors(edge, n, numCenters); + + // Bounding rectangle that determines the positioning of the rectangle that is + // cropped out of the computed mosaic to get rid of the gray borders. + MosaicRect cropping_rect; + + if (m_wb.horizontal) + { + cropping_rect.left = xLeftMost; + cropping_rect.right = xRightMost; + } + else + { + cropping_rect.top = yTopMost; + cropping_rect.bottom = yBottomMost; + } + + // Do merging and blending : + ret = DoMergeAndBlend(frames, numCenters, width, height, *imgMos, fullRect, + cropping_rect, progress, cancelComputation); + + if (m_wb.blendingType == BLEND_TYPE_HORZ) + CropFinalMosaic(*imgMos, cropping_rect); + + + m_Triangulator.freeMemory(); // note: can be called even if delaunay_alloc() wasn't successful + + imageMosaicYVU = imgMos->Y.ptr[0]; + + + if (m_wb.blendingType == BLEND_TYPE_HORZ) + { + mosaicWidth = cropping_rect.right - cropping_rect.left + 1; + mosaicHeight = cropping_rect.bottom - cropping_rect.top + 1; + } + else + { + mosaicWidth = Mwidth; + mosaicHeight = Mheight; + } + + return ret; +} + +int Blend::MosaicSizeCheck(float sizeMultiplier, float heightMultiplier) { + if (Mwidth < width || Mheight < height) { + return BLEND_RET_ERROR; + } + + if ((Mwidth * Mheight) > (width * height * sizeMultiplier)) { + return BLEND_RET_ERROR; + } + + // We won't do blending for the cases where users swing the device too much + // in the secondary direction. We use a short side to determine the + // secondary direction because users may hold the device in landsape + // or portrait. + int shortSide = min(Mwidth, Mheight); + if (shortSide > height * heightMultiplier) { + return BLEND_RET_ERROR; + } + + return BLEND_RET_OK; +} + +int Blend::FillFramePyramid(MosaicFrame *mb) +{ + ImageType mbY, mbU, mbV; + // Lay this image, centered into the temporary buffer + mbY = mb->image; + mbU = mb->getU(); + mbV = mb->getV(); + + int h, w; + + for(h=0; h<height; h++) + { + ImageTypeShort yptr = m_pFrameYPyr->ptr[h]; + ImageTypeShort uptr = m_pFrameUPyr->ptr[h]; + ImageTypeShort vptr = m_pFrameVPyr->ptr[h]; + + for(w=0; w<width; w++) + { + yptr[w] = (short) ((*(mbY++)) << 3); + uptr[w] = (short) ((*(mbU++)) << 3); + vptr[w] = (short) ((*(mbV++)) << 3); + } + } + + // Spread the image through the border + PyramidShort::BorderSpread(m_pFrameYPyr, BORDER, BORDER, BORDER, BORDER); + PyramidShort::BorderSpread(m_pFrameUPyr, BORDER, BORDER, BORDER, BORDER); + PyramidShort::BorderSpread(m_pFrameVPyr, BORDER, BORDER, BORDER, BORDER); + + // Generate Laplacian pyramids + if (!PyramidShort::BorderReduce(m_pFrameYPyr, m_wb.nlevs) || !PyramidShort::BorderExpand(m_pFrameYPyr, m_wb.nlevs, -1) || + !PyramidShort::BorderReduce(m_pFrameUPyr, m_wb.nlevsC) || !PyramidShort::BorderExpand(m_pFrameUPyr, m_wb.nlevsC, -1) || + !PyramidShort::BorderReduce(m_pFrameVPyr, m_wb.nlevsC) || !PyramidShort::BorderExpand(m_pFrameVPyr, m_wb.nlevsC, -1)) + { + LOGE("Error: Could not generate Laplacian pyramids"); + return BLEND_RET_ERROR; + } + else + { + return BLEND_RET_OK; + } +} + +int Blend::DoMergeAndBlend(MosaicFrame **frames, int nsite, + int width, int height, YUVinfo &imgMos, MosaicRect &rect, + MosaicRect &cropping_rect, float &progress, bool &cancelComputation) +{ + m_pMosaicYPyr = NULL; + m_pMosaicUPyr = NULL; + m_pMosaicVPyr = NULL; + + m_pMosaicYPyr = PyramidShort::allocatePyramidPacked(m_wb.nlevs,(unsigned short)rect.Width(),(unsigned short)rect.Height(),BORDER); + m_pMosaicUPyr = PyramidShort::allocatePyramidPacked(m_wb.nlevsC,(unsigned short)rect.Width(),(unsigned short)rect.Height(),BORDER); + m_pMosaicVPyr = PyramidShort::allocatePyramidPacked(m_wb.nlevsC,(unsigned short)rect.Width(),(unsigned short)rect.Height(),BORDER); + if (!m_pMosaicYPyr || !m_pMosaicUPyr || !m_pMosaicVPyr) + { + LOGE("Error: Could not allocate pyramids for blending"); + return BLEND_RET_ERROR_MEMORY; + } + + MosaicFrame *mb; + + CSite *esite = m_AllSites + nsite; + int site_idx; + + // First go through each frame and for each mosaic pixel determine which frame it should come from + site_idx = 0; + for(CSite *csite = m_AllSites; csite < esite; csite++) + { + if(cancelComputation) + { + if (m_pMosaicVPyr) free(m_pMosaicVPyr); + if (m_pMosaicUPyr) free(m_pMosaicUPyr); + if (m_pMosaicYPyr) free(m_pMosaicYPyr); + return BLEND_RET_CANCELLED; + } + + mb = csite->getMb(); + + mb->vcrect = mb->brect; + ClipBlendRect(csite, mb->vcrect); + + ComputeMask(csite, mb->vcrect, mb->brect, rect, imgMos, site_idx); + + site_idx++; + } + + ////////// imgMos.Y, imgMos.V, imgMos.U are used as follows ////////////// + ////////////////////// THIN STRIP MODE /////////////////////////////////// + + // imgMos.Y is used to store the index of the image from which each pixel + // in the output mosaic can be read out for the thin-strip mode. Thus, + // there is no special handling for pixels around the seam. Also, imgMos.Y + // is set to 255 wherever we can't get its value from any input image e.g. + // in the gray border areas. imgMos.V and imgMos.U are set to 128 for the + // thin-strip mode. + + ////////////////////// WIDE STRIP MODE /////////////////////////////////// + + // imgMos.Y is used the same way as the thin-strip mode. + // imgMos.V is used to store the index of the neighboring image which + // should contribute to the color of an output pixel in a band around + // the seam. Thus, in this band, we will crossfade between the color values + // from the image index imgMos.Y and image index imgMos.V. imgMos.U is + // used to store the weight (multiplied by 100) that each image will + // contribute to the blending process. Thus, we start at 99% contribution + // from the first image, then go to 50% contribution from each image at + // the seam. Then, the contribution from the second image goes up to 99%. + + // For WIDE mode, set the pixel masks to guide the blender to cross-fade + // between the images on either side of each seam: + if (m_wb.stripType == STRIP_TYPE_WIDE) + { + if(m_wb.horizontal) + { + // Set the number of pixels around the seam to cross-fade between + // the two component images, + int tw = STRIP_CROSS_FADE_WIDTH_PXLS; + + // Proceed with the image index calculation for cross-fading + // only if the cross-fading width is larger than 0 + if (tw > 0) + { + for(int y = 0; y < imgMos.Y.height; y++) + { + // Since we compare two adjecant pixels to determine + // whether there is a seam, the termination condition of x + // is set to imgMos.Y.width - tw, so that x+1 below + // won't exceed the imgMos' boundary. + for(int x = tw; x < imgMos.Y.width - tw; ) + { + // Determine where the seam is... + if (imgMos.Y.ptr[y][x] != imgMos.Y.ptr[y][x+1] && + imgMos.Y.ptr[y][x] != 255 && + imgMos.Y.ptr[y][x+1] != 255) + { + // Find the image indices on both sides of the seam + unsigned char idx1 = imgMos.Y.ptr[y][x]; + unsigned char idx2 = imgMos.Y.ptr[y][x+1]; + + for (int o = tw; o >= 0; o--) + { + // Set the image index to use for cross-fading + imgMos.V.ptr[y][x - o] = idx2; + // Set the intensity weights to use for cross-fading + imgMos.U.ptr[y][x - o] = 50 + (99 - 50) * o / tw; + } + + for (int o = 1; o <= tw; o++) + { + // Set the image index to use for cross-fading + imgMos.V.ptr[y][x + o] = idx1; + // Set the intensity weights to use for cross-fading + imgMos.U.ptr[y][x + o] = imgMos.U.ptr[y][x - o]; + } + + x += (tw + 1); + } + else + { + x++; + } + } + } + } + } + else + { + // Set the number of pixels around the seam to cross-fade between + // the two component images, + int tw = STRIP_CROSS_FADE_WIDTH_PXLS; + + // Proceed with the image index calculation for cross-fading + // only if the cross-fading width is larger than 0 + if (tw > 0) + { + for(int x = 0; x < imgMos.Y.width; x++) + { + // Since we compare two adjecant pixels to determine + // whether there is a seam, the termination condition of y + // is set to imgMos.Y.height - tw, so that y+1 below + // won't exceed the imgMos' boundary. + for(int y = tw; y < imgMos.Y.height - tw; ) + { + // Determine where the seam is... + if (imgMos.Y.ptr[y][x] != imgMos.Y.ptr[y+1][x] && + imgMos.Y.ptr[y][x] != 255 && + imgMos.Y.ptr[y+1][x] != 255) + { + // Find the image indices on both sides of the seam + unsigned char idx1 = imgMos.Y.ptr[y][x]; + unsigned char idx2 = imgMos.Y.ptr[y+1][x]; + + for (int o = tw; o >= 0; o--) + { + // Set the image index to use for cross-fading + imgMos.V.ptr[y - o][x] = idx2; + // Set the intensity weights to use for cross-fading + imgMos.U.ptr[y - o][x] = 50 + (99 - 50) * o / tw; + } + + for (int o = 1; o <= tw; o++) + { + // Set the image index to use for cross-fading + imgMos.V.ptr[y + o][x] = idx1; + // Set the intensity weights to use for cross-fading + imgMos.U.ptr[y + o][x] = imgMos.U.ptr[y - o][x]; + } + + y += (tw + 1); + } + else + { + y++; + } + } + } + } + } + + } + + // Now perform the actual blending using the frame assignment determined above + site_idx = 0; + for(CSite *csite = m_AllSites; csite < esite; csite++) + { + if(cancelComputation) + { + if (m_pMosaicVPyr) free(m_pMosaicVPyr); + if (m_pMosaicUPyr) free(m_pMosaicUPyr); + if (m_pMosaicYPyr) free(m_pMosaicYPyr); + return BLEND_RET_CANCELLED; + } + + mb = csite->getMb(); + + + if(FillFramePyramid(mb)!=BLEND_RET_OK) + return BLEND_RET_ERROR; + + ProcessPyramidForThisFrame(csite, mb->vcrect, mb->brect, rect, imgMos, mb->trs, site_idx); + + progress += TIME_PERCENT_BLEND/nsite; + + site_idx++; + } + + + // Blend + PerformFinalBlending(imgMos, cropping_rect); + + if (cropping_rect.Width() <= 0 || cropping_rect.Height() <= 0) + { + LOGE("Size of the cropping_rect is invalid - (width, height): (%d, %d)", + cropping_rect.Width(), cropping_rect.Height()); + return BLEND_RET_ERROR; + } + + if (m_pMosaicVPyr) free(m_pMosaicVPyr); + if (m_pMosaicUPyr) free(m_pMosaicUPyr); + if (m_pMosaicYPyr) free(m_pMosaicYPyr); + + progress += TIME_PERCENT_FINAL; + + return BLEND_RET_OK; +} + +void Blend::CropFinalMosaic(YUVinfo &imgMos, MosaicRect &cropping_rect) +{ + int i, j, k; + ImageType yimg; + ImageType uimg; + ImageType vimg; + + + yimg = imgMos.Y.ptr[0]; + uimg = imgMos.U.ptr[0]; + vimg = imgMos.V.ptr[0]; + + k = 0; + for (j = cropping_rect.top; j <= cropping_rect.bottom; j++) + { + for (i = cropping_rect.left; i <= cropping_rect.right; i++) + { + yimg[k] = yimg[j*imgMos.Y.width+i]; + k++; + } + } + for (j = cropping_rect.top; j <= cropping_rect.bottom; j++) + { + for (i = cropping_rect.left; i <= cropping_rect.right; i++) + { + yimg[k] = vimg[j*imgMos.Y.width+i]; + k++; + } + } + for (j = cropping_rect.top; j <= cropping_rect.bottom; j++) + { + for (i = cropping_rect.left; i <= cropping_rect.right; i++) + { + yimg[k] = uimg[j*imgMos.Y.width+i]; + k++; + } + } +} + +int Blend::PerformFinalBlending(YUVinfo &imgMos, MosaicRect &cropping_rect) +{ + if (!PyramidShort::BorderExpand(m_pMosaicYPyr, m_wb.nlevs, 1) || !PyramidShort::BorderExpand(m_pMosaicUPyr, m_wb.nlevsC, 1) || + !PyramidShort::BorderExpand(m_pMosaicVPyr, m_wb.nlevsC, 1)) + { + LOGE("Error: Could not BorderExpand!"); + return BLEND_RET_ERROR; + } + + ImageTypeShort myimg; + ImageTypeShort muimg; + ImageTypeShort mvimg; + ImageType yimg; + ImageType uimg; + ImageType vimg; + + int cx = (int)imgMos.Y.width/2; + int cy = (int)imgMos.Y.height/2; + + // 2D boolean array that contains true wherever the mosaic image data is + // invalid (i.e. in the gray border). + bool **b = new bool*[imgMos.Y.height]; + + for(int j=0; j<imgMos.Y.height; j++) + { + b[j] = new bool[imgMos.Y.width]; + } + + // Copy the resulting image into the full image using the mask + int i, j; + + yimg = imgMos.Y.ptr[0]; + uimg = imgMos.U.ptr[0]; + vimg = imgMos.V.ptr[0]; + + for (j = 0; j < imgMos.Y.height; j++) + { + myimg = m_pMosaicYPyr->ptr[j]; + muimg = m_pMosaicUPyr->ptr[j]; + mvimg = m_pMosaicVPyr->ptr[j]; + + for (i = 0; i<imgMos.Y.width; i++) + { + // A final mask was set up previously, + // if the value is zero skip it, otherwise replace it. + if (*yimg <255) + { + short value = (short) ((*myimg) >> 3); + if (value < 0) value = 0; + else if (value > 255) value = 255; + *yimg = (unsigned char) value; + + value = (short) ((*muimg) >> 3); + if (value < 0) value = 0; + else if (value > 255) value = 255; + *uimg = (unsigned char) value; + + value = (short) ((*mvimg) >> 3); + if (value < 0) value = 0; + else if (value > 255) value = 255; + *vimg = (unsigned char) value; + + b[j][i] = false; + + } + else + { // set border color in here + *yimg = (unsigned char) 96; + *uimg = (unsigned char) 128; + *vimg = (unsigned char) 128; + + b[j][i] = true; + } + + yimg++; + uimg++; + vimg++; + myimg++; + muimg++; + mvimg++; + } + } + + if(m_wb.horizontal) + { + //Scan through each row and increment top if the row contains any gray + for (j = 0; j < imgMos.Y.height; j++) + { + for (i = cropping_rect.left; i < cropping_rect.right; i++) + { + if (b[j][i]) + { + break; // to next row + } + } + + if (i == cropping_rect.right) //no gray pixel in this row! + { + cropping_rect.top = j; + break; + } + } + + //Scan through each row and decrement bottom if the row contains any gray + for (j = imgMos.Y.height-1; j >= 0; j--) + { + for (i = cropping_rect.left; i < cropping_rect.right; i++) + { + if (b[j][i]) + { + break; // to next row + } + } + + if (i == cropping_rect.right) //no gray pixel in this row! + { + cropping_rect.bottom = j; + break; + } + } + } + else // Vertical Mosaic + { + //Scan through each column and increment left if the column contains any gray + for (i = 0; i < imgMos.Y.width; i++) + { + for (j = cropping_rect.top; j < cropping_rect.bottom; j++) + { + if (b[j][i]) + { + break; // to next column + } + } + + if (j == cropping_rect.bottom) //no gray pixel in this column! + { + cropping_rect.left = i; + break; + } + } + + //Scan through each column and decrement right if the column contains any gray + for (i = imgMos.Y.width-1; i >= 0; i--) + { + for (j = cropping_rect.top; j < cropping_rect.bottom; j++) + { + if (b[j][i]) + { + break; // to next column + } + } + + if (j == cropping_rect.bottom) //no gray pixel in this column! + { + cropping_rect.right = i; + break; + } + } + + } + + RoundingCroppingSizeToMultipleOf8(cropping_rect); + + for(int j=0; j<imgMos.Y.height; j++) + { + delete b[j]; + } + + delete b; + + return BLEND_RET_OK; +} + +void Blend::RoundingCroppingSizeToMultipleOf8(MosaicRect &rect) { + int height = rect.bottom - rect.top + 1; + int residue = height & 7; + rect.bottom -= residue; + + int width = rect.right - rect.left + 1; + residue = width & 7; + rect.right -= residue; +} + +void Blend::ComputeMask(CSite *csite, BlendRect &vcrect, BlendRect &brect, MosaicRect &rect, YUVinfo &imgMos, int site_idx) +{ + PyramidShort *dptr = m_pMosaicYPyr; + + int nC = m_wb.nlevsC; + int l = (int) ((vcrect.lft - rect.left)); + int b = (int) ((vcrect.bot - rect.top)); + int r = (int) ((vcrect.rgt - rect.left)); + int t = (int) ((vcrect.top - rect.top)); + + if (vcrect.lft == brect.lft) + l = (l <= 0) ? -BORDER : l - BORDER; + else if (l < -BORDER) + l = -BORDER; + + if (vcrect.bot == brect.bot) + b = (b <= 0) ? -BORDER : b - BORDER; + else if (b < -BORDER) + b = -BORDER; + + if (vcrect.rgt == brect.rgt) + r = (r >= dptr->width) ? dptr->width + BORDER - 1 : r + BORDER; + else if (r >= dptr->width + BORDER) + r = dptr->width + BORDER - 1; + + if (vcrect.top == brect.top) + t = (t >= dptr->height) ? dptr->height + BORDER - 1 : t + BORDER; + else if (t >= dptr->height + BORDER) + t = dptr->height + BORDER - 1; + + // Walk the Region of interest and populate the pyramid + for (int j = b; j <= t; j++) + { + int jj = j; + double sj = jj + rect.top; + + for (int i = l; i <= r; i++) + { + int ii = i; + // project point and then triangulate to neighbors + double si = ii + rect.left; + + double dself = hypotSq(csite->getVCenter().x - si, csite->getVCenter().y - sj); + int inMask = ((unsigned) ii < imgMos.Y.width && + (unsigned) jj < imgMos.Y.height) ? 1 : 0; + + if(!inMask) + continue; + + // scan the neighbors to see if this is a valid position + unsigned char mask = (unsigned char) 255; + SEdgeVector *ce; + int ecnt; + for (ce = csite->getNeighbor(), ecnt = csite->getNumNeighbors(); ecnt--; ce++) + { + double d1 = hypotSq(m_AllSites[ce->second].getVCenter().x - si, + m_AllSites[ce->second].getVCenter().y - sj); + if (d1 < dself) + { + break; + } + } + + if (ecnt >= 0) continue; + + imgMos.Y.ptr[jj][ii] = (unsigned char)site_idx; + } + } +} + +void Blend::ProcessPyramidForThisFrame(CSite *csite, BlendRect &vcrect, BlendRect &brect, MosaicRect &rect, YUVinfo &imgMos, double trs[3][3], int site_idx) +{ + // Put the Region of interest (for all levels) into m_pMosaicYPyr + double inv_trs[3][3]; + inv33d(trs, inv_trs); + + // Process each pyramid level + PyramidShort *sptr = m_pFrameYPyr; + PyramidShort *suptr = m_pFrameUPyr; + PyramidShort *svptr = m_pFrameVPyr; + + PyramidShort *dptr = m_pMosaicYPyr; + PyramidShort *duptr = m_pMosaicUPyr; + PyramidShort *dvptr = m_pMosaicVPyr; + + int dscale = 0; // distance scale for the current level + int nC = m_wb.nlevsC; + for (int n = m_wb.nlevs; n--; dscale++, dptr++, sptr++, dvptr++, duptr++, svptr++, suptr++, nC--) + { + int l = (int) ((vcrect.lft - rect.left) / (1 << dscale)); + int b = (int) ((vcrect.bot - rect.top) / (1 << dscale)); + int r = (int) ((vcrect.rgt - rect.left) / (1 << dscale) + .5); + int t = (int) ((vcrect.top - rect.top) / (1 << dscale) + .5); + + if (vcrect.lft == brect.lft) + l = (l <= 0) ? -BORDER : l - BORDER; + else if (l < -BORDER) + l = -BORDER; + + if (vcrect.bot == brect.bot) + b = (b <= 0) ? -BORDER : b - BORDER; + else if (b < -BORDER) + b = -BORDER; + + if (vcrect.rgt == brect.rgt) + r = (r >= dptr->width) ? dptr->width + BORDER - 1 : r + BORDER; + else if (r >= dptr->width + BORDER) + r = dptr->width + BORDER - 1; + + if (vcrect.top == brect.top) + t = (t >= dptr->height) ? dptr->height + BORDER - 1 : t + BORDER; + else if (t >= dptr->height + BORDER) + t = dptr->height + BORDER - 1; + + // Walk the Region of interest and populate the pyramid + for (int j = b; j <= t; j++) + { + int jj = (j << dscale); + double sj = jj + rect.top; + + for (int i = l; i <= r; i++) + { + int ii = (i << dscale); + // project point and then triangulate to neighbors + double si = ii + rect.left; + + int inMask = ((unsigned) ii < imgMos.Y.width && + (unsigned) jj < imgMos.Y.height) ? 1 : 0; + + if(inMask && imgMos.Y.ptr[jj][ii] != site_idx && + imgMos.V.ptr[jj][ii] != site_idx && + imgMos.Y.ptr[jj][ii] != 255) + continue; + + // Setup weights for cross-fading + // Weight of the intensity already in the output pixel + double wt0 = 0.0; + // Weight of the intensity from the input pixel (current frame) + double wt1 = 1.0; + + if (m_wb.stripType == STRIP_TYPE_WIDE) + { + if(inMask && imgMos.Y.ptr[jj][ii] != 255) + { + // If not on a seam OR pyramid level exceeds + // maximum level for cross-fading. + if((imgMos.V.ptr[jj][ii] == 128) || + (dscale > STRIP_CROSS_FADE_MAX_PYR_LEVEL)) + { + wt0 = 0.0; + wt1 = 1.0; + } + else + { + wt0 = 1.0; + wt1 = ((imgMos.Y.ptr[jj][ii] == site_idx) ? + (double)imgMos.U.ptr[jj][ii] / 100.0 : + 1.0 - (double)imgMos.U.ptr[jj][ii] / 100.0); + } + } + } + + // Project this mosaic point into the original frame coordinate space + double xx, yy; + + MosaicToFrame(inv_trs, si, sj, xx, yy); + + if (xx < 0.0 || yy < 0.0 || xx > width - 1.0 || yy > height - 1.0) + { + if(inMask) + { + imgMos.Y.ptr[jj][ii] = 255; + wt0 = 0.0f; + wt1 = 1.0f; + } + } + + xx /= (1 << dscale); + yy /= (1 << dscale); + + + int x1 = (xx >= 0.0) ? (int) xx : (int) floor(xx); + int y1 = (yy >= 0.0) ? (int) yy : (int) floor(yy); + + // Final destination in extended pyramid +#ifndef LINEAR_INTERP + if(inSegment(x1, sptr->width, BORDER-1) && + inSegment(y1, sptr->height, BORDER-1)) + { + double xfrac = xx - x1; + double yfrac = yy - y1; + dptr->ptr[j][i] = (short) (wt0 * dptr->ptr[j][i] + .5 + + wt1 * ciCalc(sptr, x1, y1, xfrac, yfrac)); + if (dvptr >= m_pMosaicVPyr && nC > 0) + { + duptr->ptr[j][i] = (short) (wt0 * duptr->ptr[j][i] + .5 + + wt1 * ciCalc(suptr, x1, y1, xfrac, yfrac)); + dvptr->ptr[j][i] = (short) (wt0 * dvptr->ptr[j][i] + .5 + + wt1 * ciCalc(svptr, x1, y1, xfrac, yfrac)); + } + } +#else + if(inSegment(x1, sptr->width, BORDER) && inSegment(y1, sptr->height, BORDER)) + { + int x2 = x1 + 1; + int y2 = y1 + 1; + double xfrac = xx - x1; + double yfrac = yy - y1; + double y1val = sptr->ptr[y1][x1] + + (sptr->ptr[y1][x2] - sptr->ptr[y1][x1]) * xfrac; + double y2val = sptr->ptr[y2][x1] + + (sptr->ptr[y2][x2] - sptr->ptr[y2][x1]) * xfrac; + dptr->ptr[j][i] = (short) (y1val + yfrac * (y2val - y1val)); + + if (dvptr >= m_pMosaicVPyr && nC > 0) + { + y1val = suptr->ptr[y1][x1] + + (suptr->ptr[y1][x2] - suptr->ptr[y1][x1]) * xfrac; + y2val = suptr->ptr[y2][x1] + + (suptr->ptr[y2][x2] - suptr->ptr[y2][x1]) * xfrac; + + duptr->ptr[j][i] = (short) (y1val + yfrac * (y2val - y1val)); + + y1val = svptr->ptr[y1][x1] + + (svptr->ptr[y1][x2] - svptr->ptr[y1][x1]) * xfrac; + y2val = svptr->ptr[y2][x1] + + (svptr->ptr[y2][x2] - svptr->ptr[y2][x1]) * xfrac; + + dvptr->ptr[j][i] = (short) (y1val + yfrac * (y2val - y1val)); + } + } +#endif + else + { + clipToSegment(x1, sptr->width, BORDER); + clipToSegment(y1, sptr->height, BORDER); + + dptr->ptr[j][i] = (short) (wt0 * dptr->ptr[j][i] + 0.5 + + wt1 * sptr->ptr[y1][x1] ); + if (dvptr >= m_pMosaicVPyr && nC > 0) + { + dvptr->ptr[j][i] = (short) (wt0 * dvptr->ptr[j][i] + + 0.5 + wt1 * svptr->ptr[y1][x1] ); + duptr->ptr[j][i] = (short) (wt0 * duptr->ptr[j][i] + + 0.5 + wt1 * suptr->ptr[y1][x1] ); + } + } + } + } + } +} + +void Blend::MosaicToFrame(double trs[3][3], double x, double y, double &wx, double &wy) +{ + double X, Y, z; + if (m_wb.theta == 0.0) + { + X = x; + Y = y; + } + else if (m_wb.horizontal) + { + double alpha = x * m_wb.direction / m_wb.width; + double length = (y - alpha * m_wb.correction) * m_wb.direction + m_wb.radius; + double deltaTheta = m_wb.theta * alpha; + double sinTheta = sin(deltaTheta); + double cosTheta = sqrt(1.0 - sinTheta * sinTheta) * m_wb.direction; + X = length * sinTheta + m_wb.x; + Y = length * cosTheta + m_wb.y; + } + else + { + double alpha = y * m_wb.direction / m_wb.width; + double length = (x - alpha * m_wb.correction) * m_wb.direction + m_wb.radius; + double deltaTheta = m_wb.theta * alpha; + double sinTheta = sin(deltaTheta); + double cosTheta = sqrt(1.0 - sinTheta * sinTheta) * m_wb.direction; + Y = length * sinTheta + m_wb.y; + X = length * cosTheta + m_wb.x; + } + z = ProjZ(trs, X, Y, 1.0); + wx = ProjX(trs, X, Y, z, 1.0); + wy = ProjY(trs, X, Y, z, 1.0); +} + +void Blend::FrameToMosaic(double trs[3][3], double x, double y, double &wx, double &wy) +{ + // Project into the intermediate Mosaic coordinate system + double z = ProjZ(trs, x, y, 1.0); + double X = ProjX(trs, x, y, z, 1.0); + double Y = ProjY(trs, x, y, z, 1.0); + + if (m_wb.theta == 0.0) + { + // No rotation, then this is all we need to do. + wx = X; + wy = Y; + } + else if (m_wb.horizontal) + { + double deltaX = X - m_wb.x; + double deltaY = Y - m_wb.y; + double length = sqrt(deltaX * deltaX + deltaY * deltaY); + double deltaTheta = asin(deltaX / length); + double alpha = deltaTheta / m_wb.theta; + wx = alpha * m_wb.width * m_wb.direction; + wy = (length - m_wb.radius) * m_wb.direction + alpha * m_wb.correction; + } + else + { + double deltaX = X - m_wb.x; + double deltaY = Y - m_wb.y; + double length = sqrt(deltaX * deltaX + deltaY * deltaY); + double deltaTheta = asin(deltaY / length); + double alpha = deltaTheta / m_wb.theta; + wy = alpha * m_wb.width * m_wb.direction; + wx = (length - m_wb.radius) * m_wb.direction + alpha * m_wb.correction; + } +} + + + +// Clip the region of interest as small as possible by using the Voronoi edges of +// the neighbors +void Blend::ClipBlendRect(CSite *csite, BlendRect &brect) +{ + SEdgeVector *ce; + int ecnt; + for (ce = csite->getNeighbor(), ecnt = csite->getNumNeighbors(); ecnt--; ce++) + { + // calculate the Voronoi bisector intersection + const double epsilon = 1e-5; + double dx = (m_AllSites[ce->second].getVCenter().x - m_AllSites[ce->first].getVCenter().x); + double dy = (m_AllSites[ce->second].getVCenter().y - m_AllSites[ce->first].getVCenter().y); + double xmid = m_AllSites[ce->first].getVCenter().x + dx/2.0; + double ymid = m_AllSites[ce->first].getVCenter().y + dy/2.0; + double inter; + + if (dx > epsilon) + { + // neighbor is on right + if ((inter = m_wb.roundoffOverlap + xmid - dy * (((dy >= 0.0) ? brect.bot : brect.top) - ymid) / dx) < brect.rgt) + brect.rgt = inter; + } + else if (dx < -epsilon) + { + // neighbor is on left + if ((inter = -m_wb.roundoffOverlap + xmid - dy * (((dy >= 0.0) ? brect.bot : brect.top) - ymid) / dx) > brect.lft) + brect.lft = inter; + } + if (dy > epsilon) + { + // neighbor is above + if ((inter = m_wb.roundoffOverlap + ymid - dx * (((dx >= 0.0) ? brect.lft : brect.rgt) - xmid) / dy) < brect.top) + brect.top = inter; + } + else if (dy < -epsilon) + { + // neighbor is below + if ((inter = -m_wb.roundoffOverlap + ymid - dx * (((dx >= 0.0) ? brect.lft : brect.rgt) - xmid) / dy) > brect.bot) + brect.bot = inter; + } + } +} + +void Blend::FrameToMosaicRect(int width, int height, double trs[3][3], BlendRect &brect) +{ + // We need to walk the perimeter since the borders can be bent. + brect.lft = brect.bot = 2e30; + brect.rgt = brect.top = -2e30; + double xpos, ypos; + double lasty = height - 1.0; + double lastx = width - 1.0; + int i; + + for (i = width; i--;) + { + + FrameToMosaic(trs, (double) i, 0.0, xpos, ypos); + ClipRect(xpos, ypos, brect); + FrameToMosaic(trs, (double) i, lasty, xpos, ypos); + ClipRect(xpos, ypos, brect); + } + for (i = height; i--;) + { + FrameToMosaic(trs, 0.0, (double) i, xpos, ypos); + ClipRect(xpos, ypos, brect); + FrameToMosaic(trs, lastx, (double) i, xpos, ypos); + ClipRect(xpos, ypos, brect); + } +} + +void Blend::SelectRelevantFrames(MosaicFrame **frames, int frames_size, + MosaicFrame **relevant_frames, int &relevant_frames_size) +{ + MosaicFrame *first = frames[0]; + MosaicFrame *last = frames[frames_size-1]; + MosaicFrame *mb; + + double fxpos = first->trs[0][2], fypos = first->trs[1][2]; + + double midX = last->width / 2.0; + double midY = last->height / 2.0; + double z = ProjZ(first->trs, midX, midY, 1.0); + double firstX, firstY; + double prevX = firstX = ProjX(first->trs, midX, midY, z, 1.0); + double prevY = firstY = ProjY(first->trs, midX, midY, z, 1.0); + + relevant_frames[0] = first; // Add first frame by default + relevant_frames_size = 1; + + for (int i = 0; i < frames_size - 1; i++) + { + mb = frames[i]; + double currX, currY; + z = ProjZ(mb->trs, midX, midY, 1.0); + currX = ProjX(mb->trs, midX, midY, z, 1.0); + currY = ProjY(mb->trs, midX, midY, z, 1.0); + double deltaX = currX - prevX; + double deltaY = currY - prevY; + double center2centerDist = sqrt(deltaY * deltaY + deltaX * deltaX); + + if (fabs(deltaX) > STRIP_SEPARATION_THRESHOLD_PXLS || + fabs(deltaY) > STRIP_SEPARATION_THRESHOLD_PXLS) + { + relevant_frames[relevant_frames_size] = mb; + relevant_frames_size++; + + prevX = currX; + prevY = currY; + } + } + + // Add last frame by default + relevant_frames[relevant_frames_size] = last; + relevant_frames_size++; +} + +void Blend::ComputeBlendParameters(MosaicFrame **frames, int frames_size, int is360) +{ + // For FULL and PAN modes, we do not unwarp the mosaic into a rectangular coordinate system + // and so we set the theta to 0 and return. + if (m_wb.blendingType != BLEND_TYPE_CYLPAN && m_wb.blendingType != BLEND_TYPE_HORZ) + { + m_wb.theta = 0.0; + return; + } + + MosaicFrame *first = frames[0]; + MosaicFrame *last = frames[frames_size-1]; + MosaicFrame *mb; + + double lxpos = last->trs[0][2], lypos = last->trs[1][2]; + double fxpos = first->trs[0][2], fypos = first->trs[1][2]; + + // Calculate warp to produce proper stitching. + // get x, y displacement + double midX = last->width / 2.0; + double midY = last->height / 2.0; + double z = ProjZ(first->trs, midX, midY, 1.0); + double firstX, firstY; + double prevX = firstX = ProjX(first->trs, midX, midY, z, 1.0); + double prevY = firstY = ProjY(first->trs, midX, midY, z, 1.0); + + double arcLength, lastTheta; + m_wb.theta = lastTheta = arcLength = 0.0; + + // Step through all the frames to compute the total arc-length of the cone + // swept while capturing the mosaic (in the original conical coordinate system). + for (int i = 0; i < frames_size; i++) + { + mb = frames[i]; + double currX, currY; + z = ProjZ(mb->trs, midX, midY, 1.0); + currX = ProjX(mb->trs, midX, midY, z, 1.0); + currY = ProjY(mb->trs, midX, midY, z, 1.0); + double deltaX = currX - prevX; + double deltaY = currY - prevY; + + // The arcLength is computed by summing the lengths of the chords + // connecting the pairwise projected image centers of the input image frames. + arcLength += sqrt(deltaY * deltaY + deltaX * deltaX); + + if (!is360) + { + double thisTheta = asin(mb->trs[1][0]); + m_wb.theta += thisTheta - lastTheta; + lastTheta = thisTheta; + } + + prevX = currX; + prevY = currY; + } + + // Stretch this to end at the proper alignment i.e. the width of the + // rectangle is determined by the arcLength computed above and the cone + // sector angle is determined using the rotation of the last frame. + m_wb.width = arcLength; + if (is360) m_wb.theta = asin(last->trs[1][0]); + + // If there is no rotation, we're done. + if (m_wb.theta != 0.0) + { + double dx = prevX - firstX; + double dy = prevY - firstY; + + // If the mosaic was captured by sweeping horizontally + if (abs(lxpos - fxpos) > abs(lypos - fypos)) + { + m_wb.horizontal = 1; + // Calculate radius position to make ends exactly the same Y offset + double radiusTheta = dx / cos(3.14159 / 2.0 - m_wb.theta); + m_wb.radius = dy + radiusTheta * cos(m_wb.theta); + if (m_wb.radius < 0.0) m_wb.radius = -m_wb.radius; + } + else + { + m_wb.horizontal = 0; + // Calculate radius position to make ends exactly the same Y offset + double radiusTheta = dy / cos(3.14159 / 2.0 - m_wb.theta); + m_wb.radius = dx + radiusTheta * cos(m_wb.theta); + if (m_wb.radius < 0.0) m_wb.radius = -m_wb.radius; + } + + // Determine major direction + if (m_wb.horizontal) + { + // Horizontal strip + // m_wb.x,y record the origin of the rectangle coordinate system. + if (is360) m_wb.x = firstX; + else + { + if (lxpos - fxpos < 0) + { + m_wb.x = firstX + midX; + z = ProjZ(last->trs, 0.0, midY, 1.0); + prevX = ProjX(last->trs, 0.0, midY, z, 1.0); + prevY = ProjY(last->trs, 0.0, midY, z, 1.0); + } + else + { + m_wb.x = firstX - midX; + z = ProjZ(last->trs, last->width - 1.0, midY, 1.0); + prevX = ProjX(last->trs, last->width - 1.0, midY, z, 1.0); + prevY = ProjY(last->trs, last->width - 1.0, midY, z, 1.0); + } + } + dy = prevY - firstY; + if (dy < 0.0) m_wb.direction = 1.0; + else m_wb.direction = -1.0; + m_wb.y = firstY - m_wb.radius * m_wb.direction; + if (dy * m_wb.theta > 0.0) m_wb.width = -m_wb.width; + } + else + { + // Vertical strip + if (is360) m_wb.y = firstY; + else + { + if (lypos - fypos < 0) + { + m_wb.x = firstY + midY; + z = ProjZ(last->trs, midX, 0.0, 1.0); + prevX = ProjX(last->trs, midX, 0.0, z, 1.0); + prevY = ProjY(last->trs, midX, 0.0, z, 1.0); + } + else + { + m_wb.x = firstX - midX; + z = ProjZ(last->trs, midX, last->height - 1.0, 1.0); + prevX = ProjX(last->trs, midX, last->height - 1.0, z, 1.0); + prevY = ProjY(last->trs, midX, last->height - 1.0, z, 1.0); + } + } + dx = prevX - firstX; + if (dx < 0.0) m_wb.direction = 1.0; + else m_wb.direction = -1.0; + m_wb.x = firstX - m_wb.radius * m_wb.direction; + if (dx * m_wb.theta > 0.0) m_wb.width = -m_wb.width; + } + + // Calculate the correct correction factor + double deltaX = prevX - m_wb.x; + double deltaY = prevY - m_wb.y; + double length = sqrt(deltaX * deltaX + deltaY * deltaY); + double deltaTheta = (m_wb.horizontal) ? deltaX : deltaY; + deltaTheta = asin(deltaTheta / length); + m_wb.correction = ((m_wb.radius - length) * m_wb.direction) / + (deltaTheta / m_wb.theta); + } +} |