/*! * 2D wavelet noise * @param[in] nx,ny 解像度 */ void rxWaveletNoise::GenerateNoiseTile2(int n) { // MARK:GenerateNoiseTile2 if(g_pNoiseTileData != NULL && n == g_iNoiseTileSize) return; if(n%2) n++; // tile size must be even int sz = n*n; RXREAL *temp1 = new RXREAL[sz]; RXREAL *temp2 = new RXREAL[sz]; RXREAL *noise = new RXREAL[sz]; // Step 1. Fill the tile with random numbers in the range -1 to 1. for(int i = 0; i < n*n; ++i){ noise[i] = GaussianNoise(); temp1[i] = temp2[i] = 0; } // Steps 2 and 3. Downsample and Upsample the tile for(int iy = 0; iy < n; ++iy){ // each x row int idx = iy*n; Downsample(&noise[idx], &temp1[idx], n, 1); Upsample( &temp1[idx], &temp2[idx], n, 1); } for(int ix = 0; ix < n; ++ix){ // each y row int idx = ix; Downsample(&temp2[idx], &temp1[idx], n, n); Upsample( &temp1[idx], &temp2[idx], n, n); } // Step 4. Subtract out the coarse-scale contribution for(int i = 0; i < n*n; ++i){ noise[i] -= temp2[i]; } // Avoid even/odd variance difference by adding odd-offset version of noise to itself. int offset = n/2; if(offset%2 == 0) offset++; for(int i = 0, ix = 0; ix < n; ++ix){ for(int iy = 0; iy < n; ++iy){ temp1[i++] = noise[ModW(ix+offset, n)+ModW(iy+offset, n)*n]; } } for(int i = 0; i < n*n; ++i){ noise[i] += temp1[i]; } g_pNoiseTileData = noise; g_iNoiseTileSize = n; delete [] temp1; delete [] temp2; }
bool CRobot::UpdateSensorValue(float accuracy, bool manual_update) { if (mSimulationCount < mSensorSamplingRate / mSimulationTimeStep && !manual_update) return false; float step = TWO_PI / mSensorNum; for (UINT count = 0; count < mVectRobot.size(); count++){ ROBOT *robot = &mVectRobot.at(count); float sensor_dist = robot->Size / 2; for (int sensor_index = 0; sensor_index < mSensorNum; sensor_index++){ float min_dist = mSensorRange[MAX_VAL]; float pivot_angle = robot->Theta + sensor_index * step; // Calculate sensor position CPoint sensor_position = mGeom.Rotate(CPoint(robot->XPos, robot->YPos), sensor_dist, pivot_angle); // measure to object for (float i = 0; i <= 20; i++){ CPoint pt_max = mGeom.Rotate(sensor_position, mSensorRange[MAX_VAL], pivot_angle - mRadiationCone + i / 10 * mRadiationCone); CPoint res = mCanvas->GetLineIntersectionWithObjects(sensor_position,pt_max); float dist = mGeom.Calc2PointDist(sensor_position, res); // measure to other robot for (UINT count_ = 0; count_ < mVectRobot.size(); count_++){ ROBOT *robot_ = &mVectRobot.at(count_); if (robot != robot_){ // find intersection with other robot but itself mGeom.DoLineToEllipseIntersect(sensor_position, pt_max, CPoint(robot_->XPos - sensor_dist, robot_->YPos - sensor_dist), CPoint(robot_->XPos + sensor_dist, robot_->YPos + sensor_dist)); float dist_ = mGeom.GetIntersectDist(sensor_position); if (dist_ < dist) dist = dist_; } } if (dist < min_dist) min_dist = dist; } // Should not smaller than desired minimum value if (min_dist >= mSensorRange[MIN_VAL]) robot->SensorValue[sensor_index] = min_dist + GaussianNoise(mMeanNoise,mDevNoise); else robot->SensorValue[sensor_index] = mSensorRange[MIN_VAL]; } } mSimulationCount = 0; return true; }
/*! * 3Dノイズタイル生成 * @param[in] n タイルグリッド数 */ RXREAL* rxWaveletNoise::GenerateNoiseTile4r(int &n, int &nt) { if(n%2) n++; // tile size must be even long sz = n*n*n*nt; RXREAL *temp1 = new RXREAL[sz]; RXREAL *temp2 = new RXREAL[sz]; RXREAL *noise = new RXREAL[sz]; // Step 1. Fill the tile with random numbers in the range -1 to 1. for(int i = 0; i < sz; ++i){ noise[i] = GaussianNoise(); } // Steps 2 and 3. Downsample and Upsample the tile for(int iy = 0; iy < n; ++iy){ for(int iz = 0; iz < n; ++iz){ for(int it = 0; it < nt; ++it){ // each x row long idx = iy*n+iz*n*n+it*n*n*n; Downsample(&noise[idx], &temp1[idx], n, 1); Upsample( &temp1[idx], &temp2[idx], n, 1); } } } for(int ix = 0; ix < n; ++ix){ for(int iz = 0; iz < n; ++iz){ for(int it = 0; it < nt; ++it){ // each y row long idx = ix+iz*n*n+it*n*n*n; Downsample(&temp2[idx], &temp1[idx], n, n); Upsample( &temp1[idx], &temp2[idx], n, n); } } } for(int ix = 0; ix < n; ++ix){ for(int iy = 0; iy < n; ++iy){ for(int it = 0; it < nt; ++it){ // each z row long idx = ix+iy*n+it*n*n*n; Downsample(&temp2[idx], &temp1[idx], n, n*n); Upsample( &temp1[idx], &temp2[idx], n, n*n); } } } for(int ix = 0; ix < n; ++ix){ for(int iy = 0; iy < n; ++iy){ for(int iz = 0; iz < n; ++iz){ // each t row long idx = ix+iy*n+iz*n*n; Downsample(&temp2[idx], &temp1[idx], nt, n*n*n); Upsample( &temp1[idx], &temp2[idx], nt, n*n*n); } } } // Step 4. Subtract out the coarse-scale contribution for(int i = 0; i < sz; ++i){ noise[i] -= temp2[i]; } // Avoid even/odd variance difference by adding odd-offset version of noise to itself. int offset = n/2; if(offset%2 == 0) offset++; for(int i = 0, ix = 0; ix < n; ++ix){ for(int iy = 0; iy < n; ++iy){ for(int iz = 0; iz < n; ++iz){ for(int it = 0; it < nt; ++it){ temp1[i++] = noise[ModW(ix+offset, n)+ModW(iy+offset, n)*n+ModW(iz+offset, n)*n*n+ModW(it+offset, nt)*n*n*n]; } } } } for(int i = 0; i < sz; ++i){ noise[i] += temp1[i]; } delete [] temp1; delete [] temp2; return noise; }
/*! * * @param[in] * @return */ RXREAL* rxWaveletNoise::GenerateNoiseTile3r(int &nx, int &ny, int &nz) { if(nx%2) nx++; // tile size must be even if(ny%2) ny++; // tile size must be even if(nz%2) nz++; // tile size must be even int sz = nx*ny*nz; RXREAL *temp1 = new RXREAL[sz]; RXREAL *temp2 = new RXREAL[sz]; RXREAL *noise = new RXREAL[sz]; init_genrand((unsigned)time(NULL)); //init_genrand(1234); // Step 1. Fill the tile with random numbers in the range -1 to 1. for(int i = 0; i < sz; ++i){ noise[i] = GaussianNoise(); } // Steps 2 and 3. Downsample and Upsample the tile for(int iy = 0; iy < ny; ++iy){ for(int iz = 0; iz < nz; ++iz){ // each x row int idx = iy*nx+iz*nx*ny; Downsample(&noise[idx], &temp1[idx], nx, 1); Upsample( &temp1[idx], &temp2[idx], nx, 1); } } for(int ix = 0; ix < nx; ++ix){ for(int iz = 0; iz < nz; ++iz){ // each y row int idx = ix+iz*nx*ny; Downsample(&temp2[idx], &temp1[idx], ny, nx); Upsample( &temp1[idx], &temp2[idx], ny, nx); } } for(int ix = 0; ix < nx; ++ix){ for(int iy = 0; iy < ny; ++iy){ // each z row int idx = ix+iy*nx; Downsample(&temp2[idx], &temp1[idx], nz, nx*ny); Upsample( &temp1[idx], &temp2[idx], nz, nx*ny); } } // Step 4. Subtract out the coarse-scale contribution for(int i = 0; i < sz; ++i){ noise[i] -= temp2[i]; } // Avoid even/odd variance difference by adding odd-offset version of noise to itself. int offset = nx/2; if(offset%2 == 0) offset++; for(int i = 0, ix = 0; ix < nx; ++ix){ for(int iy = 0; iy < ny; ++iy){ for(int iz = 0; iz < nz; ++iz){ temp1[i++] = noise[ModW(ix+offset, nx)+ModW(iy+offset, ny)*nx+ModW(iz+offset, nz)*nx*ny]; } } } for(int i = 0; i < sz; ++i){ noise[i] += temp1[i]; } delete [] temp1; delete [] temp2; return noise; }