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]);
}
}
}
}
