Vector3iSet GraspSet::intersection(const Vector3iSet& set1, const Vector3iSet& set2) const { if (set2.size() < set1.size()) { return intersection(set2, set1); } Vector3iSet set_out(set1.size()); for (Vector3iSet::const_iterator it = set1.begin(); it != set1.end(); it++) { if (set2.find(*it) != set2.end()) { set_out.insert(*it); } } return set_out; }
void GraspSet::calculateVoxelizedShadowVectorized(const Eigen::Matrix3Xd& points, const Eigen::Vector3d& shadow_vec, int num_shadow_points, double voxel_grid_size, Vector3iSet& shadow_set) const { double t0_set = omp_get_wtime(); const int n = points.cols() * num_shadow_points; const double voxel_grid_size_mult = 1.0 / voxel_grid_size; const double max = 1.0 / 32767.0; // Eigen::Vector3d w; for(int i = 0; i < n; i++) { const int pt_idx = i / num_shadow_points; // const Eigen::Vector3d w = (points.col(pt_idx) + ((double) fastrand() * max) * shadow_vec) * voxel_grid_size_mult; shadow_set.insert(((points.col(pt_idx) + ((double) fastrand() * max) * shadow_vec) * voxel_grid_size_mult).cast<int>()); } if (MEASURE_TIME) printf("Shadow (1 camera) calculation. Runtime: %.3f, #points: %d, num_shadow_points: %d, #shadow: %d, max #shadow: %d\n", omp_get_wtime() - t0_set, (int) points.cols(), num_shadow_points, (int) shadow_set.size(), n); // std::cout << "Calculated shadow for 1 camera. Runtime: " << omp_get_wtime() - t0_set << ", #points: " << n << "\n"; }
bool cBlockPistonHandler::CanPushBlock( const Vector3i & a_BlockPos, cWorld * a_World, bool a_RequirePushable, Vector3iSet & a_BlocksPushed, const Vector3i & a_PushDir ) { const static std::array<Vector3i, 6> pushingDirs = { { Vector3i(-1, 0, 0), Vector3i(1, 0, 0), Vector3i( 0, -1, 0), Vector3i(0, 1, 0), Vector3i( 0, 0, -1), Vector3i(0, 0, 1) } }; BLOCKTYPE currBlock; NIBBLETYPE currMeta; a_World->GetBlockTypeMeta(a_BlockPos.x, a_BlockPos.y, a_BlockPos.z, currBlock, currMeta); if (currBlock == E_BLOCK_AIR) { // Air can be pushed return true; } if (!a_RequirePushable && cBlockInfo::IsPistonBreakable(currBlock)) { // Block should not be broken, when it's not in the pushing direction return true; } if (!CanPush(currBlock, currMeta)) { // When it's not required to push this block, don't fail return !a_RequirePushable; } if (a_BlocksPushed.size() >= PISTON_MAX_PUSH_DISTANCE) { // Do not allow to push too much blocks return false; } if (!a_BlocksPushed.insert(a_BlockPos).second || cBlockInfo::IsPistonBreakable(currBlock)) { return true; // Element exist already } if (currBlock == E_BLOCK_SLIME_BLOCK) { // Try to push the other directions for (const auto & testDir : pushingDirs) { if (!CanPushBlock(a_BlockPos + testDir, a_World, false, a_BlocksPushed, a_PushDir)) { // When it's not possible for a direction, then fail return false; } } } // Try to push the block in front of this block return CanPushBlock(a_BlockPos + a_PushDir, a_World, true, a_BlocksPushed, a_PushDir); }