void ApproximateValueManager::Initialize( BroadcastMultiReceiver::Ptr inputs, ros::NodeHandle& ph ) { WriteLock lock( _mutex ); _receiver = inputs; unsigned int inputDim = _receiver->GetDim(); ros::NodeHandle vh( ph.resolveName( "approximator" ) ); _value.Initialize( inputDim, vh ); _getParamServer = ph.advertiseService( "get_params", &ApproximateValueManager::GetParamsCallback, this ); _setParamServer = ph.advertiseService( "set_params", &ApproximateValueManager::SetParamsCallback, this ); std::string valueName; GetParamRequired( ph, "value_name", valueName ); register_lookup_target( ph, valueName ); ValueInfo info; info.inputDim = _receiver->GetDim(); info.paramQueryService = ph.resolveName( "get_params" ); info.paramSetService = ph.resolveName( "set_params" ); GetParamRequired( vh, "", info.approximatorInfo ); if( !_infoManager.WriteMemberInfo( valueName, info, true, ros::Duration( 10.0 ) ) ) { throw std::runtime_error( "Could not write value info!" ); } }
vec3 TerrainNoise::getNormal(float x, float y, float eps) const { float ha = getHauteur2(x,y); float g = getHauteur2(x-eps,y), d = getHauteur2(x+eps,y), b = getHauteur2(x,y+eps), h = getHauteur2(x,y-eps); vec3 vg(-eps, 0, g-ha), vd(eps, 0, d-ha), vb(0, eps, b-ha), vh(0, -eps, h-ha); float distg = length(vg), distd = length(vd), distb = length(vb), disth = length(vh); vec3 v1 = cross(vg,vh), v2 = cross(vh,vd), v3 = cross(vd,vb), v4 = cross(vb,vg); v1 = normalize(v1); v2 = normalize(v2); v3 = normalize(v3); v4 = normalize(v4); vec3 normale = v1*distg*disth + v2*disth*distd + v3*distd*distb + v4*distb*distg; return normalize(normale); }
void RBEC::initialValue() { Initial_Re phi_re_0(omega); Initial_Im phi_im_0(omega); FEMFunction <double, DIM> phi_star_0(fem_space); Operator::L2Project(phi_re_0, phi_re, Operator::LOCAL_LEAST_SQUARE, 3); Operator::L2Project(phi_im_0, phi_im, Operator::LOCAL_LEAST_SQUARE, 3); int n_dof = fem_space.n_dof(); for (int i = 0; i < n_dof;++i) phi_star_0(i) = phi_re(i); double L2Phi_0 = Functional::L2Norm(phi_re, 6); std::cout << "L2Norm = " << L2Phi_0 << std::endl; for (int i = 0; i < n_dof; ++i) { phi_re(i) /= L2Phi_0; phi_im(i) /= L2Phi_0; } FEMFunction<double, DIM> vh(fem_space); Potential V(gamma_x, gamma_y); Operator::L2Project(V, vh, Operator::LOCAL_LEAST_SQUARE, 3); vh.writeOpenDXData("V.dx"); phi_star_0.writeOpenDXData("phi0.dx"); };
gmVector3 ParticleMesh::getPosition(unsigned int i) { ParticleOpenMesh::VertexHandle vh(i); if (vh.is_valid()) { ParticleOpenMesh::Point p(mesh.point(vh)); return gmVector3(p[0],p[1],p[2]); } return gmVector3(); }
std::string Table::saveState() { QTableWidget *tablewidget = static_cast<QTableWidget*>(getQWidget()); QByteArray hhState = tablewidget->horizontalHeader()->saveState(); QByteArray vhState = tablewidget->verticalHeader()->saveState(); size_t len1 = hhState.length(), len2 = vhState.length(); std::string hh(hhState.constData(), hhState.length()), vh(vhState.constData(), vhState.length()); std::string state = boost::lexical_cast<std::string>(len1) + ":" + boost::lexical_cast<std::string>(len2) + ":" + hh + vh; return state; }
void HE_Mesh::checkModel(std::vector<ModelProblem>& problems) { for (int f = 0; f < faces.size(); f++) { if (faces[f].edge_idx[0] < 0) { problems.emplace_back("face doesn't know its edges! : " + std::to_string(f)); continue; } if (faces[f].edge_idx[1] < 0) { problems.emplace_back("face doesn't know its edges! : " + std::to_string(f)); continue; } if (faces[f].edge_idx[2] < 0) { problems.emplace_back("face doesn't know its edges! : " + std::to_string(f)); continue; } HE_FaceHandle fh(*this, f); if (fh.edge0().next() != fh.edge1()) problems.emplace_back("disconnected prev-next edges : " + std::to_string(f)); if (fh.edge1().next() != fh.edge2()) problems.emplace_back("disconnected prev-next edges : " + std::to_string(f)); if (fh.edge2().next() != fh.edge0()) problems.emplace_back("disconnected prev-next edges : " + std::to_string(f)); if (fh.edge0().face() != fh) problems.emplace_back("face's edge not connected to face : " + std::to_string(f)); if (fh.edge1().face() != fh) problems.emplace_back("face's edge not connected to face : " + std::to_string(f)); if (fh.edge2().face() != fh) problems.emplace_back("face's edge not connected to face : " + std::to_string(f)); } for (int e = 0; e < edges.size(); e++) { bool hasNull = false; if(edges[e].from_vert_idx < 0) { problems.emplace_back("edge doesn't know its vert! : " + std::to_string(e)); hasNull = true; } if(edges[e].next_edge_idx < 0) { problems.emplace_back("edge doesn't know its next edge! : " + std::to_string(e)); hasNull = true; } if(edges[e].converse_edge_idx < 0) { problems.emplace_back("edge doesn't know its converse! : " + std::to_string(e)); hasNull = true; } if(edges[e].face_idx < 0) { problems.emplace_back("edge doesn't know its face! : " + std::to_string(e)); hasNull = true; } if (hasNull) continue; HE_EdgeHandle eh(*this, e); if(eh.converse().converse() != eh) problems.emplace_back("edge isn't the converse of its converse : " + std::to_string(e)); int counter = 0; HE_EdgeHandle eh2 = eh; do { if (counter > MAX_EDGES_PER_VERTEX) { problems.emplace_back("edges don't form a circular loop! : " + std::to_string(e)); break; } // continue outer for-loop eh2 = eh2.converse().next(); counter++; } while (eh2 != eh); // dont put more code here } for (int v = 0; v < vertices.size(); v++) { if (vertices[v].someEdge_idx < 0) { problems.emplace_back("vertex doesn't know any edge! : " + std::to_string(v)); continue; } HE_VertexHandle vh(*this, v); // done for each edge! // int counter = 0; // HE_EdgeHandle eh = vh.someEdge(); // do // { // if (counter > MAX_EDGES_PER_VERTEX) { problems.emplace_back("vertex edges don't form a circular loop!"); break; } // continue outer for-loop // eh = eh.converse().next(); counter++; // } while (eh != vh.someEdge()); // // dont put more code here } }
int SerializerReader::local_version(int v) { int32_t vh(-1); recv((char *)&vh,sizeof(int32_t)); if(vh<0||vh>v) { errstr("invalid_version"); return -1; } return vh; }
THStack *getstack(TTree *nt, TString name, TString var, vector<TString> cuts,int bins = 100, float xmin = 0, float xmax = 200) { THStack *hs = new THStack(name,name); int N = cuts.size(); vector<TH1F *> vh(N); for (int i=0;i<N;i++) { vh[i] = geth(Form("%s%d",name.Data(),i),bins,xmin,xmax); vh[i]->SetFillColor(TColor::GetColorDark(i+2)); vh[i]->SetFillStyle(1001); nt->Project(vh[i]->GetName(),var.Data(),Form("weight*(%s)",cuts[i].Data()),"");//,1000); hs->Add(vh[i],"hist"); cout<<vh[i]->Integral()<<endl; } hs->SetMinimum(1E-2); return hs; }
void clarke_wright(std::vector<Vertices> vec, float** distances ){ Vertice hub = vec.front(); std::vector<Vertices> vh(vec); vh.pop_front(); std::vector<Vertice> savings; std::vector<Vertices>::iterator i; std::vector<Vertices>::iterator j; float a,b,ij; for (i= vec.begin(); i < vec.end(); ++i){ for (j = veg.begin(); j < vec.end(); ++j){ a = euclidianDistance(hub, vh[i], distances); b = euclidianDistance(hub, vh[j], distances); ij = euclidianDistance(vh[i], vh[j], distances); Vertice vi; vi.x = i; vi.y = j; } } }
int SerializerWriter::local_version(int v) { int32_t vh(v); send((char *)&vh,sizeof(int32_t)); return v; }
void ParticleMesh::setPosition(unsigned int i, gmVector3 p) { ParticleOpenMesh::VertexHandle vh(i); if (vh.is_valid()) mesh.set_point(vh,ParticleOpenMesh::Point((ParticleMeshTraits::Scaler) p[0],(ParticleMeshTraits::Scaler) p[1],(ParticleMeshTraits::Scaler) p[2])); }
// FIXME: use ct_setparam to avoid copying data void command::set_params(sybase_query &query, const QoreListNode *args, ExceptionSink *xsink) { unsigned nparams = query.param_list.size(); for (unsigned i = 0; i < nparams; ++i) { if (query.param_list[i] == 'd') continue; const AbstractQoreNode *val = args ? args->retrieve_entry(i) : NULL; CS_DATAFMT datafmt; memset(&datafmt, 0, sizeof(datafmt)); datafmt.status = CS_INPUTVALUE; datafmt.namelen = CS_NULLTERM; sprintf(datafmt.name, "@par%d", int(i + 1)); datafmt.maxlength = CS_UNUSED; datafmt.count = 1; CS_RETCODE err = CS_FAIL; if (!val || is_null(val) || is_nothing(val)) { #ifdef FREETDS // it seems to be necessary to specify a type like // this to get a null value to be bound with freetds datafmt.datatype = CS_CHAR_TYPE; datafmt.format = CS_FMT_NULLTERM; datafmt.maxlength = 1; #endif // SQL NULL value err = ct_param(m_cmd, &datafmt, 0, CS_UNUSED, -1); if (err != CS_SUCCEED) { m_conn.do_exception(xsink, "DBI:SYBASE:EXEC-ERROR", "ct_param() for 'null' failed for parameter %u with error %d", i, (int)err); return; } continue; } qore_type_t ntype = val ? val->getType() : 0; switch (ntype) { case NT_STRING: { const QoreStringNode *str = reinterpret_cast<const QoreStringNode *>(val); // ensure we bind with the proper encoding for the connection TempEncodingHelper s(str, m_conn.getEncoding(), xsink); if (!s) throw ss::Error("DBI:SYBASE:EXEC-ERROR", "encoding"); int slen = s->strlen(); datafmt.datatype = CS_CHAR_TYPE; datafmt.format = CS_FMT_NULLTERM; // NOTE: setting large sizes here like 2GB works for sybase ctlib, // not for freetds datafmt.maxlength = slen + 1; err = ct_param(m_cmd, &datafmt, (CS_VOID*)s->getBuffer(), slen, 0); break; } case NT_NUMBER: { QoreStringValueHelper vh(val); int slen = vh->strlen(); datafmt.datatype = CS_CHAR_TYPE; datafmt.format = CS_FMT_NULLTERM; datafmt.maxlength = slen + 1; err = ct_param(m_cmd, &datafmt, (CS_VOID *)vh->getBuffer(), slen, 0); break; } case NT_DATE: { const DateTimeNode *date = reinterpret_cast<const DateTimeNode *>(val); CS_DATETIME dt; ss::Conversions conv; if (conv.DateTime_to_DATETIME(date, dt, xsink)) throw ss::Error("DBI:SYBASE:EXEC-ERROR", "can't convert date"); datafmt.datatype = CS_DATETIME_TYPE; err = ct_param(m_cmd, &datafmt, &dt, sizeof(dt), 0); break; } case NT_INT: { #ifdef CS_BIGINT_TYPE datafmt.datatype = CS_BIGINT_TYPE; err = ct_param(m_cmd, &datafmt, &(const_cast<QoreBigIntNode *>(reinterpret_cast<const QoreBigIntNode *>(val))->val), sizeof(int64), 0); #else int64 ival = reinterpret_cast<const QoreBigIntNode *>(val)->val; // if it's a 32-bit integer, bind as integer if (ival <= 2147483647 && ival >= -2147483647) { datafmt.datatype = CS_INT_TYPE; CS_INT vint = ival; err = ct_param(m_cmd, &datafmt, &vint, sizeof(CS_INT), 0); } else { // bind as float CS_FLOAT fval = ival; datafmt.datatype = CS_FLOAT_TYPE; err = ct_param(m_cmd, &datafmt, &fval, sizeof(CS_FLOAT), 0); } #endif break; } case NT_BOOLEAN: { // Seems mssql doesn't like CS_BIT_TYPE for some reason. // Replacing by CS_INT_TYPE helps // // The "BIT" code is supposed to be like this: // datafmt.datatype = CS_BIT_TYPE; // err = ct_param(m_cmd, &datafmt, &bval, sizeof(bval), 0); // ... but it doesn't work CS_BIT bval = reinterpret_cast<const QoreBoolNode *>(val)->getValue(); datafmt.datatype = CS_INT_TYPE; int64 ival = bval ? 1 : 0; err = ct_param(m_cmd, &datafmt, &ival, sizeof(ival), 0); break; } case NT_FLOAT: { CS_FLOAT fval = reinterpret_cast<const QoreFloatNode *>(val)->f; datafmt.datatype = CS_FLOAT_TYPE; err = ct_param(m_cmd, &datafmt, &fval, sizeof(CS_FLOAT), 0); break; } case NT_BINARY: { const BinaryNode *b = reinterpret_cast<const BinaryNode *>(val); datafmt.datatype = CS_BINARY_TYPE; datafmt.maxlength = b->size(); datafmt.count = 1; err = ct_param(m_cmd, &datafmt, (void *)b->getPtr(), b->size(), 0); break; } default: m_conn.do_exception(xsink, "DBI:SYBASE:BIND-ERROR", "do not know how to bind values of type '%s'", val->getTypeName()); return; } // switch(ntype) if (err != CS_SUCCEED) { m_conn.do_exception(xsink, "DBI:SYBASE:EXEC-ERROR", "ct_param() for binary parameter %u failed with error", i, (int)err); } } }
void MeshConversion<SensorT>::fromPointCloud( const typename pcl::PointCloud<PointT>::ConstPtr& pc, MeshT& mesh) { typedef typename MeshT::VertexHandle VertexHandle; int rows = pc->height - 1; // last row int cols = pc->width - 1; // last column int row_offset; // [h]orizontal, [v]ertical, [l]eft, [r]ight edge check std::vector<std::vector<bool> > h(rows+1, std::vector<bool>(cols,true)); std::vector<std::vector<bool> > v(rows, std::vector<bool>(cols+1,true)); std::vector<std::vector<bool> > l(rows, std::vector<bool>(cols,true)); std::vector<std::vector<bool> > r(rows, std::vector<bool>(cols,true)); std::vector<std::vector<VertexHandle> > vh(rows+1, std::vector<VertexHandle>(cols+1)); // vertex handles /* * +--+--+ p00 h00 p01 h01 p02 * | | | v00 lr00 v01 lr01 v02 * +--+--+ p10 h10 p11 h11 p12 * | | | v10 lr10 v11 lr11 v12 * +--+--+ p20 h20 p21 h21 p22 */ // corners h.front().front() = v.front().front() = r.front().front() = false; h.front().back() = v.front().back() = l.front().back() = false; h.back().front() = v.back().front() = l.back().front() = false; h.back().back() = v.back().back() = r.back().back() = false; // first and last row for(int x = 1; x<cols; ++x) { h.front()[x-1] = false; h.front()[x ] = false; v.front()[x ] = false; l.front()[x-1] = false; r.front()[x ] = false; h.back ()[x-1] = false; h.back ()[x ] = false; v.back ()[x ] = false; r.back ()[x-1] = false; l.back ()[x ] = false; } for(int y = 1; y<rows; ++y) { // left column and right column h[y ].front() = false; v[y-1].front() = false; v[y ].front() = false; l[y-1].front() = false; r[y ].front() = false; h[y ].back() = false; v[y-1].back() = false; v[y ].back() = false; r[y-1].back() = false; l[y ].back() = false; row_offset = y*(cols+1); // iterate remaining for(int x=1; x<cols; ++x) { const PointT* p = &(*pc)[row_offset+x]; if( p->z != p->z ) { v[y-1][x ] = false; v[y ][x ] = false; h[y ][x-1] = false; h[y ][x ] = false; l[y-1][x ] = false; l[y ][x-1] = false; r[y-1][x-1] = false; r[y ][x ] = false; } else { vh[y][x] = mesh.addVertex(y*pc->width+x, *p); } } } // iterate h and v to check if edge is valid typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> > ::const_iterator pii = pc->points.begin(); typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> > ::const_iterator pij = pii + 1; // right typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> > ::const_iterator pji = pii + 1 + cols; // below typename std::vector<PointT, Eigen::aligned_allocator_indirection<PointT> > ::const_iterator pjj = pji + 1; // below right for(int y=0; y<rows; ++y) { for(int x=0; x<cols; ++x) { // check horizontal and vertical if (h[y][x]) h[y][x] = isNeighbor(pii->getVector3fMap(), pij->getVector3fMap()); if (v[y][x]) v[y][x] = isNeighbor(pii->getVector3fMap(), pji->getVector3fMap()); // check diagonal unsigned char status = (l[y][x] << 1) | r[y][x]; switch(status) { case 0b00: break; case 0b01: r[y][x] = isNeighbor(pii->getVector3fMap(), pjj->getVector3fMap()); break; case 0b10: l[y][x] = isNeighbor(pij->getVector3fMap(), pji->getVector3fMap()); break; case 0b11: if( (pij->z - pji->z) > (pii->z - pjj->z) ) { r[y][x] = false; l[y][x] = isNeighbor(pij->getVector3fMap(), pji->getVector3fMap()); } else { l[y][x] = false; r[y][x] = isNeighbor(pii->getVector3fMap(), pjj->getVector3fMap()); } break; } ++pii; ++pij; ++pji; ++pjj; } // skip the last column // note that in the very last iteration, pjj points beyond end() ++pii; ++pij; ++pji; ++pjj; } for(int y=0; y<rows; ++y) { for(int x=0; x<cols; ++x) { /* ii-ji-ij | ij-ji-jj | ii-jj-ij | ii-ji-jj * +-+ | + | +-+ | + * |/ | /| | \| | |\ * + | +-+ | + | +-+ */ if(l[y][x]) { if (h[y ][x] && v[y][x ]) mesh.addFace(vh[y][x ], vh[y+1][x], vh[y ][x+1]); if (h[y+1][x] && v[y][x+1]) mesh.addFace(vh[y][x+1], vh[y+1][x], vh[y+1][x+1]); } else if (r[y][x]) { if (h[y][x] && v[y][x+1]) mesh.addFace(vh[y][x], vh[y+1][x+1], vh[y][x+1]); if (v[y][x] && h[y+1][x]) mesh.addFace(vh[y][x], vh[y+1][x], vh[y+1][x+1]); } } } }