29 changed files with 824 additions and 958 deletions
@ -0,0 +1,35 @@ |
|||||
|
#ifndef SCANCONVERSIONALG_H |
||||
|
#define SCANCONVERSIONALG_H |
||||
|
|
||||
|
#include "./ScenarioParams.h" |
||||
|
|
||||
|
class ScanConversionAlg |
||||
|
{ |
||||
|
private: |
||||
|
static void linear(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
static void virtualConvex(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
static void convex(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
public: |
||||
|
static void scanConversion(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos); |
||||
|
|
||||
|
static void virtualScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, |
||||
|
float steering, float virtualOriginalZ, |
||||
|
float startDepth, float depth, float vcMaxTheta, |
||||
|
QVector<float>& gridPixelR, QVector<float>& gridPixelTheta); |
||||
|
|
||||
|
static void convexScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, |
||||
|
float steering, float angle, float fieldOfView, |
||||
|
float probeRadius, float startDepth, float depth, |
||||
|
QVector<float>& gridPixelR, QVector<float>& gridPixelTheta); |
||||
|
}; |
||||
|
|
||||
|
#endif // SCANCONVERSIONALG_H
|
||||
@ -0,0 +1,518 @@ |
|||||
|
#include "model/algorithms/ScanConversionAlg.h" |
||||
|
|
||||
|
#include <QtMath> |
||||
|
|
||||
|
void ScanConversionAlg::linear(Input_t params, QVector<float> &scanXPos, QVector<float> &scanZPos, |
||||
|
QVector<float> &gridPixelXPos, QVector<float> &gridPixelZPos) |
||||
|
{ |
||||
|
//just to map to X Z convention
|
||||
|
auto frameMaxXScan = params.bMaxScanAz; |
||||
|
auto frameMinXScan = params.bMinScanAz; |
||||
|
auto frameMaxZScan = params.bMaxScanAx; |
||||
|
auto frameMinZScan = params.bMinScanAx; |
||||
|
|
||||
|
auto finalPixelXNo = params.outputWidth; |
||||
|
auto finalPixelZNo = params.outputHeight; |
||||
|
|
||||
|
auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1); |
||||
|
auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1); |
||||
|
|
||||
|
gridPixelXPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
gridPixelZPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
|
||||
|
auto cap = gridPixelXPos.capacity(); |
||||
|
|
||||
|
for(auto i = 0UL; i < finalPixelZNo; i++) |
||||
|
{ |
||||
|
auto temp = frameMinZScan + i * pixelZStep; |
||||
|
for(auto j = 0UL; j < finalPixelXNo; j++) |
||||
|
{ |
||||
|
gridPixelZPos.push_back(temp / cosf(params.steering)); |
||||
|
gridPixelXPos.push_back((frameMinXScan + j * pixelXStep) - |
||||
|
(temp * tanf(params.steering))); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
for(auto i = 0; i < params.rxLineNo; i++) |
||||
|
{ |
||||
|
scanXPos.push_back(params.minScanAz + i * params.rxLineDaz); |
||||
|
} |
||||
|
|
||||
|
for(auto i = 0; i < params.rxFocusPointNo; i++) |
||||
|
{ |
||||
|
scanZPos.push_back(params.minScanAx + i * params.rxPointDax); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::virtualConvex(Input_t params, QVector<float>& scanXPos, |
||||
|
QVector<float>& scanZPos, QVector<float>& gridPixelXPos, |
||||
|
QVector<float>& gridPixelZPos) |
||||
|
{ |
||||
|
auto finalPixelXNo = params.outputWidth; |
||||
|
auto finalPixelZNo = params.outputHeight; |
||||
|
|
||||
|
auto minAbsScanAz = 0.0f; |
||||
|
if (params.bMaxScanAz > 0 && params.bMinScanAz < 0) |
||||
|
{ |
||||
|
minAbsScanAz = 0; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
minAbsScanAz = qMin(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
} |
||||
|
|
||||
|
auto maxAbsScanAz = qMax(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
auto frameMaxZScan = params.bMaxScanAx * |
||||
|
cosf(minAbsScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
auto frameMinZScan = params.bMinScanAx * |
||||
|
cosf(maxAbsScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
auto frameMaxXScan = params.bMaxScanAz + params.bMaxScanAx * |
||||
|
sinf(params.bMaxScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
auto frameMinXScan = params.bMinScanAz + params.bMaxScanAx * |
||||
|
sinf(params.bMinScanAz / params.fieldOfView * 2 * params.vcMaxTheta); |
||||
|
|
||||
|
|
||||
|
auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1); |
||||
|
auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1); |
||||
|
|
||||
|
auto pixelXPos = QVector<float>(); |
||||
|
auto pixelZPos = QVector<float>(); |
||||
|
|
||||
|
pixelXPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
pixelZPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
|
||||
|
for(auto i = 0UL; i < finalPixelZNo; i++) |
||||
|
{ |
||||
|
for(auto j = 0UL; j < finalPixelXNo; j++) |
||||
|
{ |
||||
|
pixelZPos.push_back(frameMinZScan + i * pixelZStep); |
||||
|
pixelXPos.push_back(frameMinXScan + j * pixelXStep); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
virtualScanConversion(pixelXPos, pixelZPos, finalPixelXNo, finalPixelZNo, params.steering, |
||||
|
params.virtualOriginalZ, params.startDepth, params.depth, |
||||
|
params.vcMaxTheta, gridPixelZPos, gridPixelXPos); |
||||
|
|
||||
|
//scanTheta
|
||||
|
for(auto i = 0; i < params.rxLineNo; i++) |
||||
|
{ |
||||
|
scanXPos.push_back(params.minScanAz + i * params.rxLineDaz); |
||||
|
} |
||||
|
|
||||
|
//scanR
|
||||
|
for(auto i = 0; i < params.rxFocusPointNo; i++) |
||||
|
{ |
||||
|
scanZPos.push_back(params.minScanAx + i * params.rxPointDax); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::convex(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos, |
||||
|
QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos) |
||||
|
{ |
||||
|
auto finalPixelXNo = params.outputWidth; |
||||
|
auto finalPixelZNo = params.outputHeight; |
||||
|
|
||||
|
auto frameMaxXScan = params.probe.radius * sinf(params.bMaxScanAz) + |
||||
|
params.bMaxScanAx * sinf(params.bMaxScanAz / params.fieldOfView * params.angle); |
||||
|
auto frameMinXScan = params.probe.radius * sinf(params.bMinScanAz) + |
||||
|
params.bMaxScanAx * sinf(params.bMinScanAz / params.fieldOfView * params.angle); |
||||
|
|
||||
|
auto minAbsScanAz = 0.0f; |
||||
|
if (params.bMaxScanAz > 0 && params.bMinScanAz < 0) |
||||
|
{ |
||||
|
minAbsScanAz = 0; |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
minAbsScanAz = qMin(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
} |
||||
|
auto maxAbsScanAz = qMax(abs(params.bMinScanAz), abs(params.bMaxScanAz)); |
||||
|
|
||||
|
auto frameMaxZScan = params.probe.radius * cosf(minAbsScanAz) + |
||||
|
params.bMaxScanAx * cosf(minAbsScanAz / params.fieldOfView * params.angle); |
||||
|
auto frameMinZScan = params.probe.radius * cosf(maxAbsScanAz) + |
||||
|
params.bMinScanAx * cosf(maxAbsScanAz / params.fieldOfView * params.angle); |
||||
|
|
||||
|
auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1); |
||||
|
auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1); |
||||
|
|
||||
|
auto pixelXPos = QVector<float>(); |
||||
|
auto pixelZPos = QVector<float>(); |
||||
|
|
||||
|
pixelXPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
pixelZPos.reserve(finalPixelXNo * finalPixelZNo); |
||||
|
|
||||
|
for(auto i = 0UL; i < finalPixelZNo; i++) |
||||
|
{ |
||||
|
for(auto j = 0UL; j < finalPixelXNo; j++) |
||||
|
{ |
||||
|
pixelZPos.push_back(frameMinZScan + i * pixelZStep); |
||||
|
pixelXPos.push_back(frameMinXScan + j * pixelXStep); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
convexScanConversion(pixelXPos, pixelZPos, finalPixelXNo, finalPixelZNo, params.steering, |
||||
|
params.angle, params.fieldOfView, params.probe.radius, params.startDepth, |
||||
|
params.depth, gridPixelZPos, gridPixelXPos); |
||||
|
|
||||
|
|
||||
|
//scanTheta
|
||||
|
for(auto i = 0; i < params.rxLineNo; i++) |
||||
|
{ |
||||
|
scanXPos.push_back(params.minScanAz + i * params.rxLineDaz + params.probe.radius); |
||||
|
} |
||||
|
|
||||
|
//scanR
|
||||
|
for(auto i = 0; i < params.rxFocusPointNo; i++) |
||||
|
{ |
||||
|
scanZPos.push_back(params.minScanAx + i * params.rxPointDax); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::scanConversion(Input_t params, QVector<float>& scanXPos, |
||||
|
QVector<float>& scanZPos, QVector<float>& gridPixelXPos, |
||||
|
QVector<float>& gridPixelZPos) |
||||
|
{ |
||||
|
//convex
|
||||
|
if(!params.probe.linear) |
||||
|
{ |
||||
|
convex(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos); |
||||
|
} |
||||
|
//virtual convex
|
||||
|
else if(params.virtualConvex) |
||||
|
{ |
||||
|
virtualConvex(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos); |
||||
|
} |
||||
|
//linear
|
||||
|
else |
||||
|
{ |
||||
|
linear(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::virtualScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, float steering, |
||||
|
float virtualOriginalZ, float startDepth, float depth, |
||||
|
float vcMaxTheta, QVector<float>& gridPixelR, |
||||
|
QVector<float>& gridPixelTheta) |
||||
|
{ |
||||
|
for(auto i = 0U; i < width * height; i++) |
||||
|
{ |
||||
|
auto x = pixelXPos[i]; |
||||
|
auto z = pixelZPos[i]; |
||||
|
|
||||
|
auto pixelTheta = atanf(x / (z + virtualOriginalZ)); |
||||
|
|
||||
|
if(pixelTheta >= -vcMaxTheta - abs(steering) && |
||||
|
pixelTheta <= vcMaxTheta + abs(steering)) |
||||
|
{ |
||||
|
if(steering == 0.0f) |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - virtualOriginalZ * tanf(pixelTheta), 2) + |
||||
|
powf(z, 2)); |
||||
|
|
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(gridPixelAx); |
||||
|
gridPixelTheta.push_back(pixelTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto strTan = tanf(steering); |
||||
|
auto a = virtualOriginalZ * strTan; |
||||
|
auto b = x * strTan + virtualOriginalZ + z; |
||||
|
auto c = x - z * strTan; |
||||
|
auto interceptTheta = atanf((b + sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a)); |
||||
|
if(interceptTheta > vcMaxTheta || interceptTheta < -vcMaxTheta) |
||||
|
{ |
||||
|
interceptTheta = atanf((b - sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a)); |
||||
|
if(interceptTheta > vcMaxTheta || interceptTheta < -vcMaxTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = |
||||
|
sqrtf(powf(x - virtualOriginalZ * tanf(interceptTheta), 2) + |
||||
|
powf(z, 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = |
||||
|
sqrtf(powf(x - virtualOriginalZ * tanf(interceptTheta), 2) + |
||||
|
powf(z, 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
}//steering = 0
|
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
/*************************************************************************************************/ |
||||
|
void ScanConversionAlg::convexScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos, |
||||
|
uint width, uint height, float steering, float angle, |
||||
|
float fieldOfView, float probeRadius, float startDepth, |
||||
|
float depth, QVector<float>& gridPixelR, |
||||
|
QVector<float>& gridPixelTheta) |
||||
|
{ |
||||
|
auto virtualOriginalZ = probeRadius * |
||||
|
(cosf(fieldOfView / 2) - sinf(fieldOfView / 2) / tanf(angle / 2)); |
||||
|
auto virtualOriginalZ2 = powf(virtualOriginalZ, 2); |
||||
|
|
||||
|
auto maxR = probeRadius + depth; |
||||
|
auto minR = probeRadius - virtualOriginalZ + startDepth; |
||||
|
|
||||
|
auto minTheta = -angle / 2 - abs(steering); |
||||
|
auto maxTheta = angle / 2 + abs(steering); |
||||
|
|
||||
|
auto maxInterceptTheta = fieldOfView / 2; |
||||
|
auto radius2 = powf(probeRadius, 2); |
||||
|
|
||||
|
if(steering == 0.0f) |
||||
|
{ |
||||
|
for (auto i = 0U; i < width * height; i++) |
||||
|
{ |
||||
|
auto x = pixelXPos[i]; |
||||
|
auto z = pixelZPos[i]; |
||||
|
|
||||
|
auto pixelTheta = atan2f(x, z - virtualOriginalZ); |
||||
|
auto pixelR = sqrtf(powf(x, 2) + powf(z - virtualOriginalZ, 2)); |
||||
|
|
||||
|
if(pixelR >= minR && pixelR <= maxR && pixelTheta >= minTheta && pixelTheta <= maxTheta) |
||||
|
{ |
||||
|
auto interceptTheta = 0.0f; |
||||
|
auto interceptX = 0.0f; |
||||
|
auto interceptZ = 0.0f; |
||||
|
auto alpha = 0.0f; |
||||
|
auto beta = 0.0f; |
||||
|
|
||||
|
if(x == 0.0f) |
||||
|
interceptTheta = 0; |
||||
|
else |
||||
|
{ |
||||
|
alpha = virtualOriginalZ; |
||||
|
beta = (virtualOriginalZ - z) / x; |
||||
|
interceptX = (alpha * beta + |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
interceptZ = alpha - beta * interceptX; |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
} |
||||
|
|
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
interceptX = (alpha * beta - |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
interceptZ = alpha - beta * interceptX; |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
|
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
for (auto i = 0U; i < width * height; i++) |
||||
|
{ |
||||
|
auto x = pixelXPos[i]; |
||||
|
auto z = pixelZPos[i]; |
||||
|
|
||||
|
auto pixelTheta = atan2f(x, z - virtualOriginalZ); |
||||
|
auto pixelR = sqrtf(powf(x, 2) + powf(z - virtualOriginalZ, 2)); |
||||
|
|
||||
|
if(pixelR >= minR && pixelR <= maxR && pixelTheta >= minTheta && pixelTheta <= maxTheta) |
||||
|
{ |
||||
|
auto strSin = 2 * sinf(steering); |
||||
|
auto ro = pixelR / strSin; |
||||
|
auto xo = ro * cosf(steering - pixelTheta); |
||||
|
auto zo = ro * sinf(steering - pixelTheta) + virtualOriginalZ; |
||||
|
if(zo == 0.0f) |
||||
|
{ |
||||
|
auto interceptX = (radius2 - virtualOriginalZ2) / 2 / xo; |
||||
|
auto interceptZ = sqrtf(radius2 - powf(interceptX , 2)); |
||||
|
auto interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
if (interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
interceptZ = -sqrtf(radius2 - powf(interceptX , 2)); |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
if(interceptTheta > maxInterceptTheta || |
||||
|
interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = |
||||
|
sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto alpha = (radius2 - virtualOriginalZ2 + 2 * zo * virtualOriginalZ) / 2 / zo; |
||||
|
auto beta = xo / zo; |
||||
|
|
||||
|
auto interceptX = (alpha * beta + |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
auto interceptZ = alpha - beta * interceptX; |
||||
|
auto interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
|
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
interceptX = (alpha * beta - |
||||
|
sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) / |
||||
|
(powf(beta, 2) + 1); |
||||
|
interceptZ = alpha - beta * interceptX; |
||||
|
interceptTheta = atan2f(interceptX, interceptZ); |
||||
|
if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta) |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) + |
||||
|
powf(z - probeRadius * cosf(interceptTheta), 2)); |
||||
|
if(gridPixelAx >= startDepth && gridPixelAx <= depth) |
||||
|
{ |
||||
|
gridPixelR.push_back(probeRadius + gridPixelAx); |
||||
|
gridPixelTheta.push_back(interceptTheta); |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
{ |
||||
|
gridPixelR.push_back(0); |
||||
|
gridPixelTheta.push_back(0); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
} |
||||
Loading…
Reference in new issue