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