| 100 | } |
| 101 | |
| 102 | void Rasterization::RasterTerrian(Cloth & cloth, |
| 103 | csf::PointCloud& pc, |
| 104 | vector<double> & heightVal) { |
| 105 | |
| 106 | for (std::size_t i = 0; i < pc.size(); i++) { |
| 107 | double pc_x = pc[i].x; |
| 108 | double pc_z = pc[i].z; |
| 109 | |
| 110 | double deltaX = pc_x - cloth.origin_pos.f[0]; |
| 111 | double deltaZ = pc_z - cloth.origin_pos.f[2]; |
| 112 | int col = int(deltaX / cloth.step_x + 0.5); |
| 113 | int row = int(deltaZ / cloth.step_y + 0.5); |
| 114 | |
| 115 | if ((col >= 0) && (row >= 0)) { |
| 116 | Particle *pt = cloth.getParticle(col, row); |
| 117 | pt->correspondingLidarPointList.push_back(i); |
| 118 | double pc2particleDist = SQUARE_DIST( |
| 119 | pc_x, pc_z, |
| 120 | pt->getPos().f[0], |
| 121 | pt->getPos().f[2] |
| 122 | ); |
| 123 | |
| 124 | if (pc2particleDist < pt->tmpDist) { |
| 125 | pt->tmpDist = pc2particleDist; |
| 126 | pt->nearestPointHeight = pc[i].y; |
| 127 | pt->nearestPointIndex = i; |
| 128 | } |
| 129 | } |
| 130 | } |
| 131 | heightVal.resize(cloth.getSize()); |
| 132 | |
| 133 | #pragma omp parallel for |
| 134 | for (int i = 0; i < cloth.getSize(); i++) { |
| 135 | Particle *pcur = cloth.getParticle1d(i); |
| 136 | double nearestHeight = pcur->nearestPointHeight; |
| 137 | |
| 138 | if (nearestHeight > MIN_INF) { |
| 139 | heightVal[i] = nearestHeight; |
| 140 | } else { |
| 141 | heightVal[i] = findHeightValByScanline(pcur, cloth); |
| 142 | } |
| 143 | } |
| 144 | } |
nothing calls this directly
no test coverage detected