/*!
* 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;
}
