static void r3d(void *pvParameters)
{
rr();
while(1)
{
qq();
}
}
void operator()(d_mask * mask)
{
if ( StringMatch( mask_pos, mask->getName() ) )
{
match_mask_wmean2 qq(mean, mask, surf_pos, penalty_factor);
qq = std::for_each(surfit_surfs->begin(), surfit_surfs->end(), qq);
if (res == NULL)
res = create_boolvec();
res->push_back(qq.res);
}
};
void operator()(d_mask * mask)
{
if ( StringMatch( mask_pos, mask->getName() ) )
{
if (res == NULL)
res = create_boolvec();
match_mask_surf_add2 qq(weight, surf_pos, mask);
qq = std::for_each(surfit_surfs->begin(), surfit_surfs->end(), qq);
res->push_back(qq.res);
}
};
void insertParen(struct Queue * queue) {
int ops = 0;
int needed_parens = 0;
char ch;
struct Stack *s = createStack();
struct Stack *t = createStack();
while(queue->size > 0) {
push(s, qd(queue));
}
int done = 0;
while( ! done ) {
ch = peek(s);
switch(ch) {
case '+':
case '-':
case '*':
case '/':
ops++;
if (ops > needed_parens) {
done = 1;
} else {
push(t, pop(s));
}
break;
case ')':
needed_parens++;
default:
push(t, pop(s));
break;
}
if (s->size == 0 || done == 1) {
push(t, '(');
done = 1;
}
}
// drain whatever is left
while (s->size > 0) {
push(t, pop(s));
}
while (t->size > 0) {
qq(queue, pop(t));
}
return;
}
virtual void init(Voxel& voxel)
{
voxel.fib_fa.clear();
voxel.fib_fa.resize(voxel.dim.size());
voxel.fib_dir.clear();
voxel.fib_dir.resize(voxel.dim.size()*3);
md.clear();
md.resize(voxel.dim.size());
d0.clear();
d0.resize(voxel.dim.size());
d1.clear();
d1.resize(voxel.dim.size());
b_count = voxel.bvalues.size()-1;
std::vector<image::vector<3> > b_data(b_count);
//skip null
std::copy(voxel.bvectors.begin()+1,voxel.bvectors.end(),b_data.begin());
for(unsigned int index = 0; index < b_count; ++index)
b_data[index] *= std::sqrt(voxel.bvalues[index+1]);
Kt.resize(6*b_count);
{
unsigned int qmap[6] = {0 ,4 ,8 ,1 ,2 ,5 };
double qweighting[6]= {1.0,1.0,1.0,2.0,2.0,2.0};
// bxx,byy,bzz,bxy,bxz,byz
for (unsigned int i = 0,k = 0; i < b_data.size(); ++i,k+=6)
{
//qq = q qT
std::vector<float> qq(3*3);
image::matrix::product_transpose(b_data[i].begin(),b_data[i].begin(),qq.begin(),
image::dyndim(3,1),image::dyndim(3,1));
/*
q11 q15 q19 2*q12 2*q13 2*q16
q21 q25 q29 2*q22 2*q23 2*q26
K = | ... |
*/
for (unsigned int col = 0,index = i; col < 6; ++col,index+=b_count)
Kt[index] = qq[qmap[col]]*qweighting[col];
}
}
iKtK.resize(6*6);
iKtK_pivot.resize(6);
image::matrix::product_transpose(Kt.begin(),Kt.begin(),iKtK.begin(),
image::dyndim(6,b_count),image::dyndim(6,b_count));
image::matrix::lu_decomposition(iKtK.begin(),iKtK_pivot.begin(),image::dyndim(6,6));
}
int main()
{
Zyw::vector<int> k(9, 6);
Zyw::vector<int> qq(10);
std::cout << qq.capacity() << std::endl;
std::cout<<k.size()<<std::endl;
k.clear();
for (auto qq : k)
{
std::cout << qq << std::endl;
}
system("pause");
return 0;
}
bool limadb::deletelist (const statstring &listid)
{
dbquery q (DB);
q.select (tlists["id"]);
q.where (tlists["address"] == listid.sval());
value res = q.exec ();
if (! res.count()) return false;
dbquery qq (DB);
qq.deletefrom (tmembers).where (tmembers["listid"] == res[0]["id"].ival());
qq.execvoid ();
dbquery qqq (DB);
qqq.deletefrom (tlists).where (tlists["address"] == listid.sval());
return qqq.execvoid ();
}
void CSimuVertexRingObj::updateRotationQuaternionForAllElements(const unsigned int tm, const bool needQuat)
{
//compute the rotation for each vertex
const int BUFFERLEN = 20000;
int failbuffer[BUFFERLEN];
int i, c = 0;
assert(m_nVRingElementCount==m_nVertexCount);
const Vector3d *pVertex = &m_pVertInfo[0].m_pos;
const int stride = sizeof(CSimuEntity::VertexInfo);
for (i=0; i<m_nVRingElementCount; i++){
CVertexRingElement& elm = m_pVRingElement[i];
if (!elm.computeRotationQuaternionSolidOrShell(0, pVertex, stride)){
failbuffer[c++] = i;
elm.m_rotTime = 0;
}
else{
elm.m_rotTime = tm;
}
}
//make up those failed vertices;
assert(c<BUFFERLEN);
for (i=0; i<c; i++){
const int vid = failbuffer[i];
CVertexRingElement& elm = m_pVRingElement[vid];
Vector4d q(0,0,0,0);
int count = 0;
for (int j=0; j<elm.getRodNumber(); j++){
const int v = elm.m_pVertexRingNode[j].m_nVertexID;
CVertexRingElement& elm1 = m_pVRingElement[v];
const unsigned int tm1 = elm1.m_rotTime;
if (tm1 == tm){
q += elm1.m_quat;
count ++;
}
}
if (count>0){
Quaternion qq(q[0], q[1], q[2], q[3]);
qq.normalize();
elm.m_quat = Vector4d(qq[0], qq[1], qq[2], qq[3]);
}
}
}
int main() {
const int N = 1000;
const unsigned int K = 10;
Tree tree;
Random_points_iterator rpit(4,1000.0);
for(int i = 0; i < N; i++){
tree.insert(*rpit++);
}
Point_d pp(0.1,0.1,0.1,0.1);
Point_d qq(0.2,0.2,0.2,0.2);
Iso_box_d query(pp,qq);
Distance tr_dist;
Neighbor_search N1(tree, query, 5, 10.0, false); // eps=10.0, nearest=false
std::cout << "For query rectangle = [0.1, 0.2]^4 " << std::endl
<< "the " << K << " approximate furthest neighbors are: " << std::endl;
for (Neighbor_search::iterator it = N1.begin();it != N1.end();it++) {
std::cout << " Point " << it->first << " at distance " << tr_dist.inverse_of_transformed_distance(it->second) << std::endl;
}
return 0;
}
main()
{
int n,k;
printf("Podaj mi calkowite n : ");
scanf("%i" , &n);
printf("Podaj mi calkowite k : ");
scanf("%i" , &k);
if( k >= 0 && n >= k )
{
printf("\nWwynik : %i" , qq(n,k));
}
else
{
printf("Dane nie spelniaja warunku 0 <= k <= n" );
}
printf("\n");
}
boolvec * area_setName(const char * new_name, const char * pos)
{
match_area_setName qq(new_name, pos);
qq = std::for_each(surfit_areas->begin(), surfit_areas->end(), qq);
return qq.res;
};
boolvec * area_save(const char * filename, const char * pos)
{
match_area_save qq(filename, pos);
qq = std::for_each(surfit_areas->begin(), surfit_areas->end(), qq);
return qq.res;
};
boolvec * area_write(const char * filename, const char * pos, const char * delimiter)
{
match_area_write qq(filename, pos, delimiter);
qq = std::for_each(surfit_areas->begin(), surfit_areas->end(), qq);
return qq.res;
};
boolvec * surface_geq(const char * pos, REAL penalty_factor)
{
match_surface_geq qq(pos, penalty_factor);
qq = std::for_each(surfit_surfs->begin(), surfit_surfs->end(), qq);
return qq.res;
};
void assemble ( double adiag[], double aleft[], double arite[], double f[],
double h[], int indx[], int nl, int node[], int nu, int nquad, int nsub,
double ul, double ur, double xn[], double xquad[] )
{
double aij;
double he;
int i;
int ie;
int ig;
int il;
int iq;
int iu;
int jg;
int jl;
int ju;
double phii;
double phiix;
double phij;
double phijx;
double x;
double xleft;
double xquade;
double xrite;
/*
Zero out the arrays that hold the coefficients of the matrix
and the right hand side.
*/
for ( i = 0; i < nu; i++ )
{
f[i] = 0.0;
}
for ( i = 0; i < nu; i++ )
{
adiag[i] = 0.0;
}
for ( i = 0; i < nu; i++ )
{
aleft[i] = 0.0;
}
for ( i = 0; i < nu; i++ )
{
arite[i] = 0.0;
}
/*
For interval number IE,
*/
for ( ie = 0; ie < nsub; ie++ )
{
he = h[ie];
xleft = xn[node[0+ie*2]];
xrite = xn[node[1+ie*2]];
/*
consider each quadrature point IQ,
*/
for ( iq = 0; iq < nquad; iq++ )
{
xquade = xquad[ie];
/*
and evaluate the integrals associated with the basis functions
for the left, and for the right nodes.
*/
for ( il = 1; il <= nl; il++ )
{
ig = node[il-1+ie*2];
iu = indx[ig] - 1;
if ( 0 <= iu )
{
phi ( il, xquade, &phii, &phiix, xleft, xrite );
f[iu] = f[iu] + he * ff ( xquade ) * phii;
/*
Take care of boundary nodes at which U' was specified.
*/
if ( ig == 0 )
{
x = 0.0;
f[iu] = f[iu] - pp ( x ) * ul;
}
else if ( ig == nsub )
{
x = 1.0;
f[iu] = f[iu] + pp ( x ) * ur;
}
/*
Evaluate the integrals that take a product of the basis
function times itself, or times the other basis function
that is nonzero in this interval.
*/
for ( jl = 1; jl <= nl; jl++ )
{
jg = node[jl-1+ie*2];
ju = indx[jg] - 1;
phi ( jl, xquade, &phij, &phijx, xleft, xrite );
aij = he * ( pp ( xquade ) * phiix * phijx
+ qq ( xquade ) * phii * phij );
/*
If there is no variable associated with the node, then it's
a specified boundary value, so we multiply the coefficient
times the specified boundary value and subtract it from the
right hand side.
*/
if ( ju < 0 )
{
if ( jg == 0 )
{
f[iu] = f[iu] - aij * ul;
}
else if ( jg == nsub )
{
f[iu] = f[iu] - aij * ur;
}
}
/*
Otherwise, we add the coefficient we've just computed to the
diagonal, or left or right entries of row IU of the matrix.
*/
else
{
if ( iu == ju )
{
adiag[iu] = adiag[iu] + aij;
}
else if ( ju < iu )
{
aleft[iu] = aleft[iu] + aij;
}
else
{
arite[iu] = arite[iu] + aij;
}
}
}
}
}
}
}
return;
}
boolvec * mask_add(REAL value, REAL weight, const char * pos)
{
match_mask_add qq(weight, value, pos);
qq = std::for_each(surfit_masks->begin(), surfit_masks->end(), qq);
return qq.res;
};
boolvec * surface_add(REAL weight, const char * pos)
{
match_surface_add qq(weight, pos);
qq = std::for_each(surfit_surfs->begin(), surfit_surfs->end(), qq);
return qq.res;
};
boolvec * surface(const char * pos)
{
match_surface qq(pos);
qq = std::for_each(surfit_surfs->begin(), surfit_surfs->end(), qq);
return qq.res;
};
void TrackThread::run()
{
VideoCapture caputure;
// string WindowNameOutput="window";
Mat frame;
Mat output;
//emit Mess(QString("test"),QString("camera open filed"));
//return ;
caputure.open(0);
if(!caputure.isOpened())
{
emit Messquit(QString("warning"),QString("you don't have a camera, please connect one"));
//md->mess("warning","camera open failed");
return;
}
// namedWindow(WindowNameOutput);
// cvWaitKey(1000);
Sleep(1000);
while(nRunFlag){
if(!caputure.read(frame))
{
emit Messquit(QString("warning"),QString("please connect camera"));
//md->mess("warning","camera read failed");
break;
}
flip(frame,frame,1);
int re=process (frame,output);
//控制
//if(md->ready){
if(re==1){
emit moveup();
}
//md->moveUp();
else if(re==2){
emit movedown();
}
//md->moveDown();
else if(re==3){
emit moveleft();
}
//md->moveLeft();
else if (re==4){
emit moveright();
}
//md->moveRight();
//}
// imshow(WindowNameOutput,output);
// 显示
cvtColor(output, output, CV_BGR2RGB);
QImage qq((unsigned char*)output.data,output.cols, output.rows,output.step, QImage::Format_RGB888);
// md->setimg(QPixmap::fromImage(qq));
emit setImg(QPixmap::fromImage(qq));
// if(md->exitt)
// break;
Sleep(50);
// cvWaitKey(1000);
}
caputure.release();
// destroyWindow (WindowNameOutput);
}
boolvec * trend_add(REAL weight, REAL D1, REAL D2, const char * pos)
{
match_trend_add qq(weight, D1, D2, pos);
qq = std::for_each(surfit_surfs->begin(), surfit_surfs->end(), qq);
return qq.res;
};
boolvec * mask_surf_geq(const char * surf_pos, const char * mask_pos, REAL mult)
{
match_mask_surf_geq qq(surf_pos, mask_pos, mult);
qq = std::for_each(surfit_masks->begin(), surfit_masks->end(), qq);
return qq.res;
};
boolvec * mask_surf_add(const char * surf_pos, REAL weight, const char * mask_pos)
{
match_mask_surf_add qq(weight, surf_pos, mask_pos);
qq = std::for_each(surfit_masks->begin(), surfit_masks->end(), qq);
return qq.res;
};
intvec * area_getId(const char * pos)
{
match_area_getId qq(pos);
qq = std::for_each(surfit_areas->begin(), surfit_areas->end(), qq);
return qq.res;
};
int main ( void )
{
double r = qq();
printf("answer is %f %d\n", r, (int)r );
return 0;
}
boolvec * area_invert(const char * pos)
{
match_area_invert qq(pos);
qq = std::for_each(surfit_areas->begin(), surfit_areas->end(), qq);
return qq.res;
};
void assemble (){
int offset = 0;
int slaveSizeNU = nu / numprocs;
/* MASTER WORK */
if(rank == MASTER){
int masterSizeNU = slaveSizeNU + (nu % numprocs);
double aij;
double he;
int i;
int ie;
int ig;
int il;
int iq;
int iu;
int jg;
int jl;
int ju;
double phii;
double phiix;
double phij;
double phijx;
double x;
double xleft;
double xquade;
double xrite;
/* set offset to end of master block */
offset = masterSizeNU;
/* send offsets to slaves */
for (int i = 1; i < numprocs; i++)
{
MPI_Send(&offset,1,MPI_INT,i,110,MPI_COMM_WORLD);
offset += slaveSizeNU;
}
/* master does its work with openmp */
#pragma omp parallel for
for ( i = 0; i < masterSizeNU; i++ )
{
f[i] = 0.0;
adiag[i] = 0.0;
aleft[i] = 0.0;
arite[i] = 0.0;
}
/* set offset to end of master block */
offset = masterSizeNU;
/* master receives data from slaves */
for (int i = 1; i < numprocs; i++)
{
MPI_Recv(&f[offset],slaveSizeNU,MPI_DOUBLE,i,111,MPI_COMM_WORLD,&status);
MPI_Recv(&adiag[offset],slaveSizeNU,MPI_DOUBLE,i,112,MPI_COMM_WORLD,&status);
MPI_Recv(&aleft[offset],slaveSizeNU,MPI_DOUBLE,i,113,MPI_COMM_WORLD,&status);
MPI_Recv(&arite[offset],slaveSizeNU,MPI_DOUBLE,i,114,MPI_COMM_WORLD,&status);
offset += masterSizeNU;
}
/*
For interval number IE,
*/
for ( ie = 0; ie < NSUB; ie++ )
{
he = h[ie];
xleft = xn[node[0+ie*2]];
xrite = xn[node[1+ie*2]];
/*
consider each quadrature point IQ,
*/
for ( iq = 0; iq < nquad; iq++ )
{
xquade = xquad[ie];
/*
and evaluate the integrals associated with the basis functions
for the left, and for the right nodes.
*/
for ( il = 1; il <= NL; il++ )
{
ig = node[il-1+ie*2];
iu = indx[ig] - 1;
if ( 0 <= iu )
{
phi ( il, xquade, &phii, &phiix, xleft, xrite );
f[iu] = f[iu] + he * ff ( xquade ) * phii;
/*
Take care of boundary nodes at which U' was specified.
*/
if ( ig == 0 )
{
x = 0.0;
f[iu] = f[iu] - pp ( x ) * ul;
}
else if ( ig == NSUB )
{
x = 1.0;
f[iu] = f[iu] + pp ( x ) * ur;
}
/*
Evaluate the integrals that take a product of the basis
function times itself, or times the other basis function
that is nonzero in this interval.
*/
for ( jl = 1; jl <= NL; jl++ )
{
jg = node[jl-1+ie*2];
ju = indx[jg] - 1;
phi ( jl, xquade, &phij, &phijx, xleft, xrite );
aij = he * ( pp ( xquade ) * phiix * phijx
+ qq ( xquade ) * phii * phij );
/*
If there is no variable associated with the node, then it's
a specified boundary value, so we multiply the coefficient
times the specified boundary value and subtract it from the
right hand side.
*/
if ( ju < 0 )
{
if ( jg == 0 )
{
f[iu] = f[iu] - aij * ul;
}
else if ( jg == NSUB )
{
f[iu] = f[iu] - aij * ur;
}
}
/*
Otherwise, we add the coefficient we've just computed to the
diagonal, or left or right entries of row IU of the matrix.
*/
else
{
if ( iu == ju )
{
adiag[iu] = adiag[iu] + aij;
}
else if ( ju < iu )
{
aleft[iu] = aleft[iu] + aij;
}
else
{
arite[iu] = arite[iu] + aij;
}
}
}
// printf("%d\n",jl );
}
}
}
}
}
/* SLAVE WORK */
if(rank != MASTER){
/* receive offset */
MPI_Recv(&offset,1,MPI_INT,MASTER,110,MPI_COMM_WORLD,&status);
/* slave does its work with openmp */
#pragma omp parallel for
for (int i = offset; i < (offset + slaveSizeNU); i++ )
{
f[i] = 0.0;
adiag[i] = 0.0;
aleft[i] = 0.0;
arite[i] = 0.0;
}
/* slave sends data to master */
MPI_Send(&f[offset],slaveSizeNU,MPI_DOUBLE,MASTER,111,MPI_COMM_WORLD);
MPI_Send(&adiag[offset],slaveSizeNU,MPI_DOUBLE,MASTER,112,MPI_COMM_WORLD);
MPI_Send(&aleft[offset],slaveSizeNU,MPI_DOUBLE,MASTER,113,MPI_COMM_WORLD);
MPI_Send(&arite[offset],slaveSizeNU,MPI_DOUBLE,MASTER,114,MPI_COMM_WORLD);
}
}
boolvec * mask_wmean(REAL mean, const char * mask_pos, const char * surf_pos, REAL penalty_factor)
{
match_mask_wmean qq(mean, mask_pos, surf_pos, penalty_factor);
qq = std::for_each(surfit_masks->begin(), surfit_masks->end(), qq);
return qq.res;
};
boolvec * mask_geq(REAL value, const char * mask_pos, REAL penalty_factor)
{
match_mask_geq qq(value, mask_pos, penalty_factor);
qq = std::for_each(surfit_masks->begin(), surfit_masks->end(), qq);
return qq.res;
};
int getPath() {
getKey = true;
if (feedbackKey || goalKey || mapKey){
if (feedbackKey) ROS_INFO("huai le1");
if (goalKey) ROS_INFO("huai le2");
if (mapKey) ROS_INFO("huai le3");
getKey = false;
return 0;
}
int idd = 0;
int mapData[height*width];
std::queue<tpoint> path, q;
std::vector<tpoint> tpath;
PATH.clear();
tpath.clear();
while (!path.empty()) path.pop();
while (!q.empty()) q.pop();
nowPoint = 0;
for (int i = 0; i < width * height; i ++) {
mapData[i] = mapDataInit[i];
}
double z[3];
tf::Quaternion qq(feedbackData.orientation.x, feedbackData.orientation.y, feedbackData.orientation.z, feedbackData.orientation.w);
tf::Matrix3x3 m(qq);
m.getRPY(z[0], z[1], z[2]);
double x1 , x2 , x;
double y1, y2, y;
ROS_INFO("hua");
for (int i = 90; i < 450; i ++) {
x1 = scanData[i] * cos((i - 270) * 0.00999999977648 + z[2]);
y1 = scanData[i] * sin((i - 270) * 0.00999999977648 + z[2]);
x2 = 0.215 * cos(z[2]);
y2 = 0.215 * sin(z[2]);
x = feedbackData.position.x + x1 + x2;
y = feedbackData.position.y + y1 + y2;
printf("%f %f %f\n", x, y, z[2]);
for (int ii = int(y / pointSize) + Start.y - 3; ii < int(y / pointSize) + Start.y + 3; ii ++) {
for (int jj = int(x / pointSize) + Start.x - 3; jj < int(x / pointSize) + Start.x + 3; jj ++) {
mapData[ii * width + jj] = 100;
}
}
}
ROS_INFO("start getPath");
bitathome_navigation_control::MyMap mapDataPub;
for (int i = 0; i < height * width; i ++)
mapDataPub.data.push_back(mapData[i]);
mapShow_pub.publish(mapDataPub);
/*
for (int i = 0; i < width * height; i ++) {
printf("%d," , mapData[i]);
}
printf("\n");
exit(0);
*/
ROS_INFO("start getPath");
tpoint tNow, temp;
tNow.x = Now.x;
tNow.y = Now.y;
tNow.idd = idd;
idd ++;
q.push(tNow);
tpath.push_back(tNow);
while (!q.empty()){
temp = q.front();
q.pop();
if (temp.x == Goal.x and temp.y == Goal.y){
while (true) {
path.push(tpath[temp.idd]);
temp = tpath[temp.pa];
if (temp.idd == 0){
break;
}
}
//printf("path len is %d\n", path.size());
int next = -1;
while (!path.empty()) {
if (path.front().ddir != next) {
PATH.push_back(path.front());
next = path.front().ddir;
}
path.pop();
}
reverse(PATH.begin(),PATH.end());
printf("PATH len is %d\n", (int)PATH.size());
for (int i = 0; i < PATH.size(); i ++)
printf("路径点%d: %d %d %f\n",i , PATH[i].x, PATH[i].y, euler_angles[PATH[i].ddir]);
ROS_INFO("getPath over");
getKey = false;
int pathLen = PATH.size();
if (pathLen == 1) {
bitathome_navigation_control::MyPoint ret;
ret.x = (PATH[nowPoint].x - Start.x) * pointSize;
ret.y = (PATH[nowPoint].y - Start.y) * pointSize;
ret.z = goalData.z;
ret.say = goalData.say;
goalPoint_pub.publish(ret);
} else {
bitathome_navigation_control::MyPoint ret;
ret.x = (PATH[nowPoint].x - Start.x) * pointSize;
ret.y = (PATH[nowPoint].y - Start.y) * pointSize;
ret.z = euler_angles[PATH[nowPoint].ddir];
ret.say = "";
goalPoint_pub.publish(ret);
}
return pathLen;
}
for (int i = 0; i < 8; i ++) {
tpoint tson;
tson.x = temp.x + dirs[i][0];
tson.y = temp.y + dirs[i][1];
if (tson.y * width + tson.x > 0 && tson.y * width + tson.x < width* height && (mapData[tson.y * width + tson.x] == 0 || (mapData[tson.y * width + tson.x] == -1 && mapDataInit[temp.y*width+temp.x] == -1))){
tson.ddir = i;
tson.pa = temp.idd;
tson.idd = idd;
idd ++;
q.push(tson);
tpath.push_back(tson);
mapData[tson.y * width + tson.x] = 100;
}
}
}
PATH.clear();
ROS_INFO("f**k");
return 0;
}
boolvec * mask_completer_add(REAL weight, const char * mask_pos, REAL D1, REAL D2, REAL alpha, REAL w)
{
match_mask_completer_add qq(weight, mask_pos, D1, D2, alpha, w);
qq = std::for_each(surfit_masks->begin(), surfit_masks->end(), qq);
return qq.res;
};