406 lines
13 KiB
Plaintext
406 lines
13 KiB
Plaintext
#include <time.h>
|
||
#include <iostream>
|
||
#include <memory>
|
||
#include <cmath>
|
||
#include <complex>
|
||
#include <device_launch_parameters.h>
|
||
#include <cuda_runtime.h>
|
||
#include <cublas_v2.h>
|
||
#include <cuComplex.h>
|
||
|
||
#include "BaseConstVariable.h"
|
||
|
||
#include "LookTableSimulationComputer.cuh"
|
||
|
||
|
||
extern __device__ double getNumberDopplerCenterRate(double R, double r0, double r1, double r2, double r3, double r4, double reftime)
|
||
{
|
||
double t=R / LIGHTSPEED - reftime;
|
||
double dopplerCenterRate = r0 + r1*t + r2*t*t + r3*t*t*t + r4*t*t*t*t;
|
||
return dopplerCenterRate;
|
||
}
|
||
|
||
__device__ double getDopplerCenterRate(double Rx, double Ry, double Rz, double Vx, double Vy, double Vz, double fact_lamda)
|
||
{
|
||
return -2*(Rx*Vx+Ry*Vy+Rz*Vz)* fact_lamda;
|
||
}
|
||
|
||
__device__ double getPolyfitNumber(double x, double a0, double a1, double a2, double a3, double a4, double a5)
|
||
{
|
||
return a0 + a1 * x + a2 * x * x + a3 * x * x * x + a4 * x * x * x * x + a5 * x * x * x * x * x;
|
||
}
|
||
|
||
|
||
__global__ void Kernel_RDProcess_doppler(
|
||
double* demX, double* demY, double* demZ,
|
||
float* outRidx, float* outCidx,
|
||
long pixelcount,
|
||
double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
|
||
double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
|
||
double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
|
||
double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
|
||
double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
|
||
double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5,
|
||
double reftime, double r0, double r1, double r2, double r3, double r4,
|
||
double starttime, double nearRange, double farRange,
|
||
double PRF, double Fs,
|
||
double fact_lamda
|
||
) {
|
||
long idx = blockIdx.x * blockDim.x + threadIdx.x;
|
||
if (idx < pixelcount) {
|
||
double demx = demX[idx];
|
||
double demy = demY[idx];
|
||
double demz = demZ[idx];
|
||
float Rd_r = -1;
|
||
float Rd_c = -1;
|
||
double dt = 1.0 / PRF / 3;
|
||
double Spx = 0, Spy = 0, Spz = 0, Svx = 0, Svy = 0, Svz = 0;
|
||
double Rx = 0, Ry = 0, Rz = 0,R = 0;
|
||
|
||
double dp1=0, dpn1=0, dp2=0, dpn2=0;
|
||
|
||
double ti = 0;
|
||
double inct = 0;
|
||
for (long i = 0; i < 10000; i++) { // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Χ
|
||
Spx = getPolyfitNumber(ti+dt, Xp0, Xp1, Xp2, Xp3, Xp4, Xp5);
|
||
Spy = getPolyfitNumber(ti+dt, Yp0, Yp1, Yp2, Yp3, Yp4, Yp5);
|
||
Spz = getPolyfitNumber(ti+dt, Zp0, Zp1, Zp2, Zp3, Zp4, Zp5);
|
||
Svx = getPolyfitNumber(ti+dt, Xv0, Xv1, Xv2, Xv3, Xv4, Xv5);
|
||
Svy = getPolyfitNumber(ti+dt, Yv0, Yv1, Yv2, Yv3, Yv4, Yv5);
|
||
Svz = getPolyfitNumber(ti+dt, Zv0, Zv1, Zv2, Zv3, Zv4, Zv5);
|
||
|
||
Rx = Spx - demx;
|
||
Ry = Spy - demy;
|
||
Rz = Spz - demz;
|
||
R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
|
||
Rx = Rx / R;
|
||
Ry = Ry / R;
|
||
Rz = Rz / R;
|
||
|
||
dp2 = getDopplerCenterRate(Rx, Ry, Rz, Svx, Svy, Svz, fact_lamda);
|
||
dpn2 = getNumberDopplerCenterRate(R, r0, r1, r2, r3, r4, reftime);
|
||
|
||
|
||
|
||
// ti
|
||
Spx = getPolyfitNumber(ti, Xp0, Xp1, Xp2, Xp3, Xp4, Xp5);
|
||
Spy = getPolyfitNumber(ti, Yp0, Yp1, Yp2, Yp3, Yp4, Yp5);
|
||
Spz = getPolyfitNumber(ti, Zp0, Zp1, Zp2, Zp3, Zp4, Zp5);
|
||
Svx = getPolyfitNumber(ti, Xv0, Xv1, Xv2, Xv3, Xv4, Xv5);
|
||
Svy = getPolyfitNumber(ti, Yv0, Yv1, Yv2, Yv3, Yv4, Yv5);
|
||
Svz = getPolyfitNumber(ti, Zv0, Zv1, Zv2, Zv3, Zv4, Zv5);
|
||
|
||
Rx = Spx - demx;
|
||
Ry = Spy - demy;
|
||
Rz = Spz - demz;
|
||
R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
|
||
Rx = Rx / R;
|
||
Ry = Ry / R;
|
||
Rz = Rz / R;
|
||
|
||
dp1=getDopplerCenterRate(Rx, Ry, Rz, Svx, Svy, Svz, fact_lamda);
|
||
dpn1 = getNumberDopplerCenterRate(R, r0, r1, r2, r3, r4, reftime);
|
||
|
||
// iter
|
||
inct = dt * (dp2 - dpn1) / (dp1 - dp2);
|
||
|
||
//printf("ti: %10.6f\n", ti);
|
||
//printf("inct: %10.6f\n", inct);
|
||
//printf("demx: %10.6f\n", demx);
|
||
//printf("demy: %10.6f\n", demy);
|
||
//printf("demz: %10.6f\n", demz);
|
||
//printf("Rd_r: %10.6f\n", Rd_r);
|
||
//printf("Rd_c: %10.6f\n", Rd_c);
|
||
//printf("dt: %10.6f\n", dt);
|
||
//printf("Spx: %10.6f\n", Spx);
|
||
//printf("Spy: %10.6f\n", Spy);
|
||
//printf("Spz: %10.6f\n", Spz);
|
||
//printf("Svx: %10.6f\n", Svx);
|
||
//printf("Svy: %10.6f\n", Svy);
|
||
//printf("Svz: %10.6f\n", Svz);
|
||
//printf("Rx: %10.6f\n", Rx);
|
||
//printf("Ry: %10.6f\n", Ry);
|
||
//printf("Rz: %10.6f\n", Rz);
|
||
//printf("R: %10.6f\n", R);
|
||
//printf("dp1: %10.6f\n", dp1);
|
||
//printf("dpn1: %10.6f\n", dpn1);
|
||
//printf("dp2: %10.6f\n", dp2);
|
||
//printf("dpn2: %10.6f\n", dpn2);
|
||
|
||
if (abs(inct) <= dt||isnan(inct) ) {
|
||
|
||
Rd_r = (ti - starttime) * PRF;
|
||
Rd_c = Fs/LIGHTSPEED * (R - nearRange)*2;
|
||
//printf("ti: %10.6f,starttime:%10.6f,PRF:%10.6f,Rd_r:%10.6f,Rd_c:%10.6f,R:%10.6f,nearRange:%e Fs: %e\n", ti, starttime, PRF, Rd_r, Rd_c, R, nearRange,Fs);
|
||
outRidx[idx] = Rd_r;
|
||
outCidx[idx] = Rd_c;//Rd_c;
|
||
return;
|
||
}
|
||
ti = ti + inct;
|
||
}
|
||
outRidx[idx] = -1;
|
||
outCidx[idx] = -1;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
__global__ void Kernel_RDProcess_doppler_calRangeDistance(
|
||
double* demX, double* demY, double* demZ,
|
||
double* outR,
|
||
long pixelcount,
|
||
double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
|
||
double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
|
||
double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
|
||
double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
|
||
double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
|
||
double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5,
|
||
double reftime, double r0, double r1, double r2, double r3, double r4,
|
||
double starttime, double nearRange, double farRange,
|
||
double PRF, double Fs,
|
||
double fact_lamda
|
||
) {
|
||
long idx = blockIdx.x * blockDim.x + threadIdx.x;
|
||
if (idx < pixelcount) {
|
||
double demx = demX[idx];
|
||
double demy = demY[idx];
|
||
double demz = demZ[idx];
|
||
double dt = 1.0 / PRF / 3;
|
||
double Spx = 0, Spy = 0, Spz = 0, Svx = 0, Svy = 0, Svz = 0;
|
||
double Rx = 0, Ry = 0, Rz = 0, R = 0;
|
||
|
||
double dp1 = 0, dpn1 = 0, dp2 = 0, dpn2 = 0;
|
||
|
||
double ti = 0;
|
||
double inct = 0;
|
||
for (long i = 0; i < 10000; i++) { // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Χ
|
||
Spx = getPolyfitNumber(ti + dt, Xp0, Xp1, Xp2, Xp3, Xp4, Xp5);
|
||
Spy = getPolyfitNumber(ti + dt, Yp0, Yp1, Yp2, Yp3, Yp4, Yp5);
|
||
Spz = getPolyfitNumber(ti + dt, Zp0, Zp1, Zp2, Zp3, Zp4, Zp5);
|
||
Svx = getPolyfitNumber(ti + dt, Xv0, Xv1, Xv2, Xv3, Xv4, Xv5);
|
||
Svy = getPolyfitNumber(ti + dt, Yv0, Yv1, Yv2, Yv3, Yv4, Yv5);
|
||
Svz = getPolyfitNumber(ti + dt, Zv0, Zv1, Zv2, Zv3, Zv4, Zv5);
|
||
|
||
Rx = Spx - demx;
|
||
Ry = Spy - demy;
|
||
Rz = Spz - demz;
|
||
R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
|
||
Rx = Rx / R;
|
||
Ry = Ry / R;
|
||
Rz = Rz / R;
|
||
|
||
dp2 = getDopplerCenterRate(Rx, Ry, Rz, Svx, Svy, Svz, fact_lamda);
|
||
dpn2 = getNumberDopplerCenterRate(R, r0, r1, r2, r3, r4, reftime);
|
||
|
||
// ti
|
||
Spx = getPolyfitNumber(ti, Xp0, Xp1, Xp2, Xp3, Xp4, Xp5);
|
||
Spy = getPolyfitNumber(ti, Yp0, Yp1, Yp2, Yp3, Yp4, Yp5);
|
||
Spz = getPolyfitNumber(ti, Zp0, Zp1, Zp2, Zp3, Zp4, Zp5);
|
||
Svx = getPolyfitNumber(ti, Xv0, Xv1, Xv2, Xv3, Xv4, Xv5);
|
||
Svy = getPolyfitNumber(ti, Yv0, Yv1, Yv2, Yv3, Yv4, Yv5);
|
||
Svz = getPolyfitNumber(ti, Zv0, Zv1, Zv2, Zv3, Zv4, Zv5);
|
||
|
||
Rx = Spx - demx;
|
||
Ry = Spy - demy;
|
||
Rz = Spz - demz;
|
||
R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
|
||
Rx = Rx / R;
|
||
Ry = Ry / R;
|
||
Rz = Rz / R;
|
||
|
||
dp1 = getDopplerCenterRate(Rx, Ry, Rz, Svx, Svy, Svz, fact_lamda);
|
||
dpn1 = getNumberDopplerCenterRate(R, r0, r1, r2, r3, r4, reftime);
|
||
|
||
// iter
|
||
inct = dt * (dp2 - dpn1) / (dp1 - dp2);
|
||
|
||
if (abs(inct) <= dt || isnan(inct)) {
|
||
outR[idx] = R;//Rd_c;
|
||
return;
|
||
}
|
||
ti = ti + inct;
|
||
}
|
||
outR[idx] = 0;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
void RDProcess_dopplerGPU(
|
||
double* demX, double* demY, double* demZ,
|
||
float* outRidx, float* outCidx,
|
||
long rowcount, long colcount,
|
||
double starttime, double nearRange, double farRange,
|
||
double PRF, double Fs,
|
||
double fact_lamda,
|
||
double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
|
||
double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
|
||
double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
|
||
double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
|
||
double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
|
||
double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5,
|
||
double reftime, double r0, double r1, double r2, double r3, double r4
|
||
|
||
)
|
||
{
|
||
long pixelcount = rowcount * colcount;
|
||
int numBlocks = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;
|
||
Kernel_RDProcess_doppler << <numBlocks, BLOCK_SIZE >> > (
|
||
demX, demY, demZ,
|
||
outRidx, outCidx,
|
||
pixelcount,
|
||
Xp0, Yp0, Zp0, Xv0, Yv0, Zv0,
|
||
Xp1, Yp1, Zp1, Xv1, Yv1, Zv1,
|
||
Xp2, Yp2, Zp2, Xv2, Yv2, Zv2,
|
||
Xp3, Yp3, Zp3, Xv3, Yv3, Zv3,
|
||
Xp4, Yp4, Zp4, Xv4, Yv4, Zv4,
|
||
Xp5, Yp5, Zp5, Xv5, Yv5, Zv5,
|
||
reftime, r0, r1, r2, r3, r4,
|
||
starttime,nearRange, farRange,
|
||
PRF, Fs,
|
||
fact_lamda
|
||
);
|
||
PrintLasterError("RD with doppler function");
|
||
cudaDeviceSynchronize();
|
||
}
|
||
|
||
|
||
|
||
void RDProcess_dopplerGPU_InSARImagePlaneXYZR(
|
||
double* demX, double* demY, double* demZ,
|
||
double* outR,
|
||
long rowcount, long colcount,
|
||
double starttime, double nearRange, double farRange,
|
||
double PRF, double Fs,
|
||
double fact_lamda,
|
||
double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
|
||
double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
|
||
double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
|
||
double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
|
||
double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
|
||
double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5,
|
||
double reftime, double r0, double r1, double r2, double r3, double r4
|
||
|
||
)
|
||
{
|
||
long pixelcount = rowcount * colcount;
|
||
int numBlocks = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;
|
||
Kernel_RDProcess_doppler_calRangeDistance << <numBlocks, BLOCK_SIZE >> > (
|
||
demX, demY, demZ,
|
||
outR,
|
||
pixelcount,
|
||
Xp0, Yp0, Zp0, Xv0, Yv0, Zv0,
|
||
Xp1, Yp1, Zp1, Xv1, Yv1, Zv1,
|
||
Xp2, Yp2, Zp2, Xv2, Yv2, Zv2,
|
||
Xp3, Yp3, Zp3, Xv3, Yv3, Zv3,
|
||
Xp4, Yp4, Zp4, Xv4, Yv4, Zv4,
|
||
Xp5, Yp5, Zp5, Xv5, Yv5, Zv5,
|
||
reftime, r0, r1, r2, r3, r4,
|
||
starttime, nearRange, farRange,
|
||
PRF, Fs,
|
||
fact_lamda
|
||
);
|
||
PrintLasterError("RD with doppler function");
|
||
cudaDeviceSynchronize();
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
__device__ double calculateIncidenceAngle(double Rx, double Ry, double Rz, double Sx, double Sy, double Sz) {
|
||
double dotProduct = Rx * Sx + Ry * Sy + Rz * Sz;
|
||
double magnitudeR = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
|
||
double magnitudeS = sqrt(Sx * Sx + Sy * Sy + Sz * Sz);
|
||
return acos(dotProduct / (magnitudeR * magnitudeS));
|
||
}
|
||
|
||
__global__ void Kernel_RDProcess_demSloper(
|
||
double* demX, double* demY, double* demZ,
|
||
double* demSloperX, double* demSloperY, double* demSloperZ,
|
||
float* InRidx,
|
||
float* outIncAngle,
|
||
long pixelcount,
|
||
double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
|
||
double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
|
||
double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
|
||
double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
|
||
double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
|
||
double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5,
|
||
|
||
double starttime, double nearRange, double farRange,
|
||
double PRF
|
||
) {
|
||
long idx = blockIdx.x * blockDim.x + threadIdx.x;
|
||
if (idx < pixelcount) {
|
||
double demx = demX[idx];
|
||
double demy = demY[idx];
|
||
double demz = demZ[idx];
|
||
double demSloperx = demSloperX[idx];
|
||
double demSlopery = demSloperY[idx];
|
||
double demSloperz = demSloperZ[idx];
|
||
float Rd_r = InRidx[idx];
|
||
double ti = starttime + Rd_r / PRF;
|
||
double Spx = getPolyfitNumber(ti, Xp0, Xp1, Xp2, Xp3, Xp4, Xp5);
|
||
double Spy = getPolyfitNumber(ti, Yp0, Yp1, Yp2, Yp3, Yp4, Yp5);
|
||
double Spz = getPolyfitNumber(ti, Zp0, Zp1, Zp2, Zp3, Zp4, Zp5);
|
||
double Rx = Spx - demx;
|
||
double Ry = Spy - demy;
|
||
double Rz = Spz - demz;
|
||
double R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
|
||
Rx = Rx / R;
|
||
Ry = Ry / R;
|
||
Rz = Rz / R;
|
||
double incidenceAngle = calculateIncidenceAngle(Rx, Ry, Rz, demSloperx, demSlopery, demSloperz);
|
||
//printf("incangle:%f\n", incidenceAngle * r2d);
|
||
outIncAngle[idx] = incidenceAngle*r2d;
|
||
}
|
||
}
|
||
|
||
void RDProcess_demSloperGPU(
|
||
double* demX, double* demY, double* demZ,
|
||
double* demSloperX, double* demSloperY, double* demSloperZ,
|
||
float* InRidx,
|
||
float* outIncAngle,
|
||
long rowcount, long colcount,
|
||
double starttime, double nearRange, double farRange,
|
||
double PRF,
|
||
double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
|
||
double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
|
||
double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
|
||
double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
|
||
double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
|
||
double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5
|
||
|
||
) {
|
||
long pixelcount = rowcount * colcount;
|
||
int numBlocks = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;
|
||
Kernel_RDProcess_demSloper << <numBlocks, BLOCK_SIZE >> > (
|
||
demX, demY, demZ,
|
||
demSloperX, demSloperY, demSloperZ,
|
||
InRidx,
|
||
outIncAngle,
|
||
pixelcount,
|
||
Xp0, Yp0, Zp0, Xv0, Yv0, Zv0,
|
||
Xp1, Yp1, Zp1, Xv1, Yv1, Zv1,
|
||
Xp2, Yp2, Zp2, Xv2, Yv2, Zv2,
|
||
Xp3, Yp3, Zp3, Xv3, Yv3, Zv3,
|
||
Xp4, Yp4, Zp4, Xv4, Yv4, Zv4,
|
||
Xp5, Yp5, Zp5, Xv5, Yv5, Zv5,
|
||
|
||
starttime, nearRange, farRange,
|
||
PRF
|
||
);
|
||
PrintLasterError("RD with demSloper function");
|
||
cudaDeviceSynchronize();
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|