DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs != 3 || nlhs != 8) {
    mexErrMsgIdAndTxt(
        "Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs",
        "Usage [num_cnst, cnst_val, iAfun, jAvar, A, cnst_name, lb, ub] = "
        "testMultipleTimeLinearPostureConstraintmex(kinCnst, q, t)");
  }
  MultipleTimeLinearPostureConstraint* cnst =
      (MultipleTimeLinearPostureConstraint*)getDrakeMexPointer(prhs[0]);
  int n_breaks = static_cast<int>(mxGetNumberOfElements(prhs[2]));
  double* t_ptr = new double[n_breaks];
  memcpy(t_ptr, mxGetPrSafe(prhs[2]), sizeof(double) * n_breaks);
  int nq = cnst->getRobotPointer()->get_num_positions();
  Eigen::MatrixXd q(nq, n_breaks);
  if (mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks) {
    mexErrMsgIdAndTxt(
        "Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs",
        "Argument 2 must be of size nq*n_breaks");
  }
  memcpy(q.data(), mxGetPrSafe(prhs[1]), sizeof(double) * nq * n_breaks);
  int num_cnst = cnst->getNumConstraint(t_ptr, n_breaks);
  Eigen::VectorXd c(num_cnst);
  cnst->feval(t_ptr, n_breaks, q, c);
  Eigen::VectorXi iAfun;
  Eigen::VectorXi jAvar;
  Eigen::VectorXd A;
  cnst->geval(t_ptr, n_breaks, iAfun, jAvar, A);
  std::vector<std::string> cnst_names;
  cnst->name(t_ptr, n_breaks, cnst_names);
  Eigen::VectorXd lb(num_cnst);
  Eigen::VectorXd ub(num_cnst);
  cnst->bounds(t_ptr, n_breaks, lb, ub);
  Eigen::VectorXd iAfun_tmp(iAfun.size());
  Eigen::VectorXd jAvar_tmp(jAvar.size());
  for (int i = 0; i < iAfun.size(); i++) {
    iAfun_tmp(i) = (double)iAfun(i) + 1;
    jAvar_tmp(i) = (double)jAvar(i) + 1;
  }
  plhs[0] = mxCreateDoubleScalar((double)num_cnst);
  plhs[1] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[1]), c.data(), sizeof(double) * num_cnst);
  plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(), 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[2]), iAfun_tmp.data(),
         sizeof(double) * iAfun_tmp.size());
  plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(), 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[3]), jAvar_tmp.data(),
         sizeof(double) * jAvar_tmp.size());
  plhs[4] = mxCreateDoubleMatrix(A.size(), 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[4]), A.data(), sizeof(double) * A.size());
  int name_ndim = 1;
  mwSize name_dims[] = {(mwSize)num_cnst};
  plhs[5] = mxCreateCellArray(name_ndim, name_dims);
  mxArray* name_ptr;
  for (int i = 0; i < num_cnst; i++) {
    name_ptr = mxCreateString(cnst_names[i].c_str());
    mxSetCell(plhs[5], i, name_ptr);
  }
  plhs[6] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL);
  plhs[7] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[6]), lb.data(), sizeof(double) * num_cnst);
  memcpy(mxGetPrSafe(plhs[7]), ub.data(), sizeof(double) * num_cnst);
  delete[] t_ptr;
}
Ejemplo n.º 2
0
void TOSMWidget::loadNData(QString DbFileName)
{
//    TIDs multiusedNodes;

    {
        QSqlDatabase db;
        db = QSqlDatabase::addDatabase("QSQLITE");
        db.setDatabaseName(DbFileName);
        if (!db.open())
        {
            qDebug() << "!!! Failed to open database !!!";
            exit(0);
        }
        QSqlQuery q(db);
        qDebug() << QTime::currentTime().toString() << " " << "requesting ways...";
        if (!q.exec("select "
                    "w.id way, "
                    "t.value tag, "
                    "tv.value value "
                    "from "
                    "t_ways w "
                    "inner join t_ways_tags wt on w.id = wt.way "
                    "inner join t_tags t on wt.tag = t.id and (t.value in ('highway', 'oneway', 'junction')) "
                    "inner join t_tags_values tv on wt.value = tv.id"
                    ))
        {
            qDebug() << "!!! failed to recieve ways!!" << q.lastError().text();
            return;
        }
        qDebug() << QTime::currentTime().toString() << "receiving ways...";
        while (q.next())
        {
            TID w = q.value(q.record().indexOf("way")).toLongLong();
            QString t = q.value(q.record().indexOf("tag")).toString();
            QString v = q.value(q.record().indexOf("value")).toString();
            if (!nways.contains(w)) nways.insert(w, TNWay(this));
            TNWay * way = &(nways[w]);

            if (v == "motorway") { way->setRoadClass(TNWay::EW_Motorway); }
            if (v == "motorway_link") { way->setRoadClass(TNWay::EW_Motorway); way->setIsLink(true);}
            if (v == "trunk") { way->setRoadClass(TNWay::EW_Trunk); }
            if (v == "trunk_link") { way->setRoadClass(TNWay::EW_Trunk); way->setIsLink(true);}
            if (v == "primary") { way->setRoadClass(TNWay::EW_Primary); }
            if (v == "primary_link") { way->setRoadClass(TNWay::EW_Primary); way->setIsLink(true);}
            if (v == "secondary") { way->setRoadClass(TNWay::EW_Secondary); }
            if (v == "secondary_link") { way->setRoadClass(TNWay::EW_Secondary); way->setIsLink(true);}
            if (v == "tertiary") { way->setRoadClass(TNWay::EW_Tertiary); }
            if (v == "tertiary_link") { way->setRoadClass(TNWay::EW_Tertiary); way->setIsLink(true);}
            if (v == "living_street") { way->setRoadClass(TNWay::EW_LivingStreet); }
            if (v == "pedestrian") { way->setRoadClass(TNWay::EW_Pedestrian); }
            if (v == "residential") { way->setRoadClass(TNWay::EW_Residental); }
            if (v == "unclassified") { way->setRoadClass(TNWay::EW_Unclassified); }
            if (v == "service") { way->setRoadClass(TNWay::EW_Service); }
            if (v == "track") { way->setRoadClass(TNWay::EW_Track); }
            if (v == "bus_guideway") { way->setRoadClass(TNWay::EW_BusGuideway); }
            if (v == "raceway") { way->setRoadClass(TNWay::EW_Raceway); }
            if (v == "road") { way->setRoadClass(TNWay::EW_Road); }
            if (v == "path") { way->setRoadClass(TNWay::EW_Path); }
            if (v == "footway") { way->setRoadClass(TNWay::EW_Footway); }
            if (v == "bridleway") { way->setRoadClass(TNWay::EW_Bridleway); }
            if (v == "steps") { way->setRoadClass(TNWay::EW_Steps); }
            if (v == "cycleway") { way->setRoadClass(TNWay::EW_Cycleway); }
            if (v == "proposed") { way->setRoadClass(TNWay::EW_Proposed); }
            if (v == "construction") { way->setRoadClass(TNWay::EW_Construction); }
            if (v == "escape") { way->setRoadClass(TNWay::EW_Escape); }
            if (t == "oneway")
            {
                if ((v == "yes") || (v == "true") || (v == "true"))
                {
                    way->setOneWay(TNWay::OW_yes_forward);
                }
                if ((v == "-1") || (v == "reverse"))
                {
                    way->setOneWay(TNWay::OW_yes_reverce);
                }
                if (v == "no")
                {
                    way->setOneWay(TNWay::OW_no);
                }
            }
            if (t == "area")
            {
                if ((v == "yes") || (v == "true") || (v == "true"))
                {
                    way->setIsArea(true);
                }
                if ((v == "-1") || (v == "reverse"))
                {
                    way->setIsArea(true);
                }
                if (v == "no")
                {
                    way->setIsArea(false);
                }
            }
        }

        qDebug() << QTime::currentTime().toString() << "requesting nodes...";

        if (!q.exec("select "
                       "w.id way, "
                       "n.lat lat, "
                       "n.lon lon, "
                       "n.id node "
                   "from "
                       "t_ways w "
                       "inner join t_ways_tags wt on w.id = wt.way "
                       "inner join t_tags t on wt.tag = t.id and (t.value = 'highway') "
                       "inner join t_ways_nodes wn on w.id = wn.way "
                       "inner join t_nodes n on n.id = wn.node "
                       "inner join t_tags_values tv on wt.value = tv.id "
//                   "where "
//                    "tv.value not in ("
//                                    "'pedestrian',"
//                                    "'footway',"
//                                    "'path',"
//                                    "'steps',"
//                                    "'construction',"
//                                    "'cycleway',"
//                                    "'proposed',"
//                                    "'platform',"
//                                    "'bridleway',"
//                                    "'piste'"
//                                ") "
//                    "tv.value in ("
    //                "'residental',"
    //                "'service',"
    //                "'living_street',"
    //                "'road',"
//                                    "'motorway',"
//                                    "'motorway_link',"
//                                    "'trunk', "
//                                    "'trunk_link',"
//                                    "'primary',"
//                                    "'primary_link',"
//                                    "'secondary',"
//                                    "'secondary_link',"
//                                    "'tertiary_link',"
//                                    "'tertiary'"
//                                ") "

    //                   "and n.lat > 61.1 and n.lat < 61.2 and n.lon > 62.75 "
                   "order by 1"))

        {
            qDebug() << "!!! Failed to recieve nodes !!! " << q.lastError().driverText() ;
            return;
        }
        bool first = true;
        qDebug() << QTime::currentTime().toString() << " " << "receiving nodes...";
        while (q.next())
        {
    //        qDebug() << "q.next()";
            TID w = q.value(q.record().indexOf("way")).toLongLong();
            TID n = q.value(q.record().indexOf("node")).toLongLong();
            double lat = q.value(q.record().indexOf("lat")).toDouble();
            double lon = q.value(q.record().indexOf("lon")).toDouble();
            QPointF point(lon, lat);
            if (first)
            {
                first = false;
                Rect.setLeft(point.x());
                Rect.setRight(point.x());
                Rect.setTop(point.y());
                Rect.setBottom(point.y());

            }
            else
            {
    //            qDebug() << point;
                Rect.setLeft(std::min(Rect.left(), point.x()));
                Rect.setRight(std::max(Rect.right(), point.x()));
                Rect.setTop(std::min(Rect.top(), point.y()));
                Rect.setBottom(std::max(Rect.bottom(), point.y()));
            }
            if (!nways.contains(w)) nways.insert(w, TNWay(this));
            nways[w].nodes.append(n);
            if (!nnodes.contains(n))
            {
                TNNode NNode(lat, lon);
                nnodes.insert(n, NNode);
            }
            nnodes[n].lat = lat;
            nnodes[n].lon = lon;
            nnodes[n].ownedBy.insert(w);
//            if (nnodes[n].containedBy.size() > 1)
//            {
//                MyCrosses.insert(n);
//            }
        }
        db.close();
    }

//    for (TIDs::Iterator it = MyCrosses.begin(); it != MyCrosses.end(); it++)
//    {
//        for (TIDs::Iterator it_w = nnodes[*it].containedBy.begin(); it_w != nnodes[*it].containedBy.end(); it_w++)
//        {
//            nways[*it_w].
//            nways[*it_w]. (*it);
//        }
//    }



//    Rect.setBottom((Rect.top() + Rect.bottom()) / 2);
//    qDebug() << "rect " << Rect;

//    TIDs checkedNodes;
////    QMap <TID, TID> nodesToMyNodes;

//    qDebug() << QTime::currentTime().toString() << " " << "converting data...";

//    for (TIDsIter it_n = multiusedNodes.begin(); it_n != multiusedNodes.end(); it_n++)
//    {
//        TID nodeId = (*it_n);
//        TNNode *node = &nnodes[nodeId];
//        if (node->containedBy.size() > 1)
//        {
//            TID newMyNodeId = 0;
//            if (!nodesToMyNodes.contains(nodeId))
//            {
//                newMyNodeId = MyNodes.size();
//                nodesToMyNodes.insert(nodeId, newMyNodeId);
//                TNNode newMyNode(node->lat, node->lon);
//                MyNodes.push_back(newMyNode);

//            }
//            else
//            {
//                newMyNodeId = nodesToMyNodes[nodeId];
//            }
////            MyNodes.append(TNNode(node->lat, node->lon));
////            qDebug() << "!!!  NEW multinode (" << node->containedBy.size() << ") " << newMyNodeId;

//            for (TIDsIter it_w = node->containedBy.begin(); it_w != node->containedBy.end(); it_w++)
//            {
//                TNWay *way = &nways[*it_w];
////                qDebug() << " ! next way " << way->nodes << " we need " << nodeId;
////                qDebug() << "<<<";
//                int inode = way->getNodeIndex(nodeId);
////                QList <TID> newWayNodes;
//                bool addThisWay = true;
//                TNWay newMyWay(this);
//                newMyWay.nodes.append(newMyNodeId);
//                for (int i = inode - 1; i >= 0; i--)
//                {
//                    TNNode *waynode = &nnodes[way->nodes[i]];
//                    if (!nodesToMyNodes.contains(way->nodes[i]))
//                    {
//                        nodesToMyNodes.insert(way->nodes[i], MyNodes.size());
//                        TNNode newMyNode(waynode->lat, waynode->lon);
////                        qDebug() << newMyNode.lat << ", " << newMyNode.lon;
//                        MyNodes.push_back(newMyNode);
//                    }
//                    newMyWay.nodes.push_back(nodesToMyNodes[way->nodes[i]]);
//                    if (checkedNodes.contains(way->nodes[i]))
//                    {
//                        addThisWay = false;
////                        qDebug() << "already checked " << nodesToMyNodes[way->nodes[i]];
//                        break;
//                    }
//                    if (waynode->containedBy.size() > 1) break;
//                }
//                if (addThisWay && (newMyWay.nodes.size() > 1))
//                {
//                    MyWays.push_back(newMyWay);
////                    qDebug() << "++ way " << newMyWay.nodes;
//                }
//                else
//                {
////                    qDebug() << "-- way " << newMyWay.nodes << " " << (!addThisWay?"checked":"");
//                }

////                qDebug() << ">>>";
//                addThisWay = true;
//                newMyWay.nodes.clear();
//                newMyWay.nodes.append(newMyNodeId);
//                for (int i = inode + 1; i < way->nodes.size(); i++)
//                {
//                    TNNode *waynode = &nnodes[way->nodes[i]];
//                    if (!nodesToMyNodes.contains(way->nodes[i]))
//                    {
//                        nodesToMyNodes.insert(way->nodes[i], MyNodes.size());
//                        TNNode newMyNode(waynode->lat, waynode->lon);
////                        qDebug() << newMyNode.lat << ", " << newMyNode.lon;
//                        MyNodes.push_back(newMyNode);
//                    }
//                    newMyWay.nodes.push_back(nodesToMyNodes[way->nodes[i]]);
//                    if (checkedNodes.contains(way->nodes[i]))
//                    {
//                        addThisWay = false;
////                        qDebug() << "already checked " << nodesToMyNodes[way->nodes[i]];
//                        break;
//                    }
//                    if (waynode->containedBy.size() > 1) break;
//                }
//                if (addThisWay && (newMyWay.nodes.size() > 1))
//                {
//                    MyWays.push_back(newMyWay);
////                    qDebug() << "++ way " << newMyWay.nodes;
//                }
//                else
//                {
////                    qDebug() << "-- way " << newMyWay.nodes << " " << (!addThisWay?"checked":"");
//                }
//            }
//            checkedNodes.insert(nodeId);
//        }

//    }

//    for (TID i = 0; i < MyWays.size(); i++)
////    for (QList <TNWay>::iterator it_w = MyWays.begin(); it_w != MyWays.end(); it_w++)
//    {
//        TNWay *way = &MyWays[i];
//        for (QList <TID> ::Iterator it = way->nodes.begin(); it != way->nodes.end(); it++)
//        {
//            MyNodes[*it].containedBy.insert(i);
//        }
////        MyNodes[way->nodes.first()].containedBy.insert(i);
////        MyNodes[way->nodes.last()].containedBy.insert(i);
//        MyCrosses.insert(way->nodes.first());
//        MyCrosses.insert(way->nodes.last());
//    }



}
Ejemplo n.º 3
0
void Camera::rotateAroundY(float angle)
{
	Quaternionf q(0.0f, sin(-angle/2.0f), 0.0f, cos(-angle/2.0f));
	m_rotation = q * m_rotation;
	m_rotation.normalize();
}
Ejemplo n.º 4
0
bool csWaterDemo::HandleEvent (iEvent& ev)
{
  if (ev.Name == FocusGained)
  {
    hasfocus = (csCommandEventHelper::GetInfo(&ev) != 0);
    CS_ASSERT(hasfocus == true);
    int w = r3d->GetDriver2D ()->GetWidth ()/2;
    int h = r3d->GetDriver2D ()->GetHeight ()/2;
    r3d->GetDriver2D ()->SetMousePosition (w, h);
    r3d->GetDriver2D ()->SetMouseCursor (csmcNone);
  }
  else if (ev.Name == FocusLost)
  {
    hasfocus = (csCommandEventHelper::GetInfo(&ev) != 0);
    CS_ASSERT(hasfocus == false);
    r3d->GetDriver2D()->SetMouseCursor (csmcArrow);
  }
  else if (ev.Name == Frame)
  {
    waterdemo->SetupFrame ();
    return true;
  }
  else if (ev.Name == KeyboardDown)
  {
    switch (csKeyEventHelper::GetCookedCode (&ev))
    {
    case CSKEY_ESC:
      {
        csRef<iEventQueue> q (csQueryRegistry<iEventQueue> (object_reg));
        if (q) 
	  q->GetEventOutlet()->Broadcast (csevQuit (object_reg));
        return true;
      }
    case CSKEY_TAB:
      console->SetVisible (!console->GetVisible ());
      break;
    case CSKEY_SPACE:
      pushDownPoint ( ((float)rand()/(float)RAND_MAX)*(Height-1), ((float)rand()/(float)RAND_MAX)*(Width-1), 0.5f);
      break;
    case '1':
      WaveSpeed -= 0.01f;
      WaveSpeed = MAX(WaveSpeed,0);
      break;
    case '2':
      WaveSpeed += 0.01f;
      WaveSpeed = MIN(WaveSpeed,0.5f);
      break;
    case '3':
      TimeDelta -= 0.01f;
      break;
    case '4':
      TimeDelta += 0.01f;
      break;
    case '5':
      WaveLife -= 0.01f;
      break;
    case '6':
      WaveLife += 0.01f;
      break;
    }
  }
  return false;
}
Ejemplo n.º 5
0
int main(int argc, char * argv[]) {


  int NUMTHREADS=10;
  if (argc>1) NUMTHREADS=atoi(argv[1]);

  ImageProducer producer;
  Thread p1(std::ref(producer));
  p1.detach();


  Queue<Worker::value_type*> q(30);
    
  size_t stride = 4000; // shall match L1 cache
  
   __sync_lock_test_and_set(&Worker::start,NUMTHREADS+1);
  ThreadGroup threads;
  threads.reserve(NUMTHREADS);
  std::vector<Worker> workers(NUMTHREADS, Worker(q));
  for (int i=0; i<NUMTHREADS; ++i) {
    threads.push_back(Thread(std::ref(workers[i])));
  }

  // we shall wait for all threads to be ready...
  // (for timing)
  do{}while(Worker::start!=1);
   // start worker
  __sync_add_and_fetch(&Worker::start,-1);

  long long mapTime=0;
  long long reduceTime=0;
  for (int l=0; l<10;++l)
  {
    // reset queue;
    //q.reset();
 
    if ( producer.q.empty() ) std::cout << "producer empty" << std::endl;
    if ( producer.q.full() ) std::cout << "producer full" << std::endl;
    ImageProducer::value_type * image;
    producer.q.pop(image);
    //map
    long long st = rdtsc();
    ImageProducer::value_type * curr = image;
    ImageProducer::value_type * end = image+ ImageProducer::imageSize;
    while(curr<end) {
      // std::cout << curr-image << " " << (int)(*curr) << std::endl;
      q.push(curr);
      curr+=stride;
    }  
    
    // barrier
    do{} while (!q.empty());
    mapTime+= rdtsc()-st;

    // reduce
    std::vector<int> hist(256);
    st = rdtsc();
    for (int i=0; i!=NUMTHREADS;++i)
      for (int j=0; j!=256;++j) 
	hist[j]+= workers[i].hist[j];
    reduceTime+= rdtsc()-st;
    
    for (int i=0; i!=NUMTHREADS;++i) {
      std::cout << "thread "<< i << " : ";
      for (int j=0; j!=256;++j)
	std::cout << workers[i].hist[j] << " ,";
      std::cout << std::endl;
    }
   
    std::cout << "\nTotal " << l << std::endl;
    for (int j=0; j!=256;++j) 
      std::cout << hist[j] << " ,";
    std::cout << std::endl;

    delete [] image;

    // prepare new loop (actually part of reduce step)
    for (int i=0; i<NUMTHREADS; ++i) workers[i].zero();
  }

  Worker::active=false;
  q.drain();
  std::for_each(threads.begin(),threads.end(), 
		std::bind(&Thread::join,std::placeholders::_1));
  

  std::cout << "map time " << double(mapTime)/1000. << std::endl;
  std::cout << "reduce time " << double(reduceTime)/1000. << std::endl;
     
  return 0;

  }
Ejemplo n.º 6
0
void SqlDB::connectDB()
{
    db = QSqlDatabase::addDatabase("QSQLITE");          //添加sqlite类型的数据库
    db.setDatabaseName("crane.db");                            //指定数据库名为crane.s3db
   // db.setDatabaseName(":memory:");
    if ( !db.open())
    {
        QMessageBox::critical(NULL,"sqlDB","connect error");
        return;
    }
    else
    {
        //qDebug()<<"open db success";
        QSqlQuery q(db);
        /*创建基本信息表,并初始化值*/
        /*
        quint16 basicVersion;                 //信息版本
        QString craneName;            //塔吊名称(16字节)
        quint8 craneID;                    //塔机ID
        quint8 groupID;                    //塔群ID
        quint8 firmID;                      //厂商ID
        quint8 craneType;                   //塔吊类型(4bits)
        quint8 craneRate;                   //吊绳倍率(4bits)
        double coordinateX;             //X坐标(-3276.8-3276.8)
        double coordinateY;             //Y坐标(-3276.8-3276.8)
        double forearmLen;             //前臂长(0-6553.5)
        double backarmLen;            //后臂长(0-6553.5)
        double craneHeight;            //塔高(0-6553.5)
        double craneTopHeight;      // 塔顶高度(0-25.6)
        quint32 craneSectionNum;     //塔吊节数
        quint32 preSectionHeight;       //每节塔高
        quint32 firstSectionHeight;         //首节塔高
        QString craneIMEI;                      //塔机IMEI*/
        q.exec("create table BasicData(id integer primary key ,"
                                                      "basicVersion int,"    //信息版本
                                                      "craneName varchar(16),"      //塔吊名称(16字节)
                                                      "craneID int,"             //塔机ID
                                                      "groupID int,"            //塔群ID
                                                      "firmID int,"                 //厂商ID
                                                      "craneType int,"           //塔吊类型(4bits)
                                                      "craneRate int,"           //吊绳倍率(4bits)
                                                      "coordinateX double,"         //X坐标(-3276.8-3276.8)
                                                      "coordinateY double,"        //Y坐标(-3276.8-3276.8)
                                                      "forearmLen double,"         //前臂长(0-6553.5)
                                                      "backarmLen double,"         //后臂长(0-6553.5)
                                                      "craneHeight double,"         //塔高(0-6553.5)
                                                      "craneTopHeight double,"     // 塔顶高度(0-25.6)
                                                      "craneSectionNum int,"        //塔吊节数
                                                      "preSectionHeight double,"           //每节塔高
                                                      "firstSectionHeight double,"          //首节塔高
                                                      "craneIMEI varchar)");                    //塔机IMEI
        q.exec(tr("INSERT INTO BasicData VALUES(0,"
                  "3,"
                  "'电子科大',"
                  "0,"
                  "0,"
                  "0,"
                  "0,"
                  "2,"
                  "0,"
                  "0,"
                  "50,"
                  "10,"
                  "20,"
                  "10,"
                  "10,"
                  "2.5,"
                  "'0',"
                  "'8651910000')"));

        /*创建保护区信息并初始化*/
        /*
        quint16 proZoneVersion;                 //保护区信息版本(2Bytes)
        //quint8  ProZoneNum;                    //保护区个数(1Bytes)

        quint8 proZoneType;                     //保护区类型(1bit  [7])
        quint8 proZoneOrder;                    //保护区序号(3bits [6:4])
        quint8  proZoneEleNum;              //保护区元素个数(4bits [3:0])三种合成一个Byte

        QString proZoneName;                //保护区名称(16Bytes)
        quint8 proZoneID;                       //保护区ID(1Bytes)
        quint8 proBuildType;                    //保护区建筑类型(1Bytes)
        double proZoneHeight;                 //保护区高度(2Bytes)
         QList<ElementData> elementData;        //元素信息数据*/
        q.exec("create table ProZoneData(id integer primary key ,"
                                                      "proZoneVersion int,"    //信息版本
                                                      "proZoneName varchar(16),"      //保护区名称(16字节)
                                                      "proZoneID int,"           //保护区ID(1Bytes)

                                                      "proZoneType int,"             //保护区类型(1bit  [7])
                                                      "proZoneOrder int,"            //保护区序号(3bits [6:4])
                                                      "proZoneEleNum int,"                 //保护区元素个数(4bits [3:0])三种合成一个Byte

                                                      "proBuildType int,"           //保护区建筑类型(1Bytes)
                                                      "proZoneHeight double)");                    //保护区高度(2Bytes)
        /*创建保护区元素信息表*"foreign key(proZoneID) references ProZoneData)"*/
        /*
        quint8 elementType;          //元素类型(0x00点,0x01圆弧)
        double pointX;                  //X坐标((2Bytes)-3276.8-3276.8)
        double pointY;                  //Y坐标((2Bytes)-3276.8-3276.8)
        double radius;                 //圆半径((2Bytes)0-6553.5)
        double startAngle;              //起始角度((2Bytes)0-360)
        double endAngle;                //终止角度((2Bytes)0-360)*/
        q.exec("create table ElementData(id integer primary key ,"
                                                          "proZoneID int,"       //从属于保护区ID
                                                          "elementType int,"    //元素类型
                                                          "pointX double,"      //元素X坐标
                                                          "pointY double,"           //元素Y坐标
                                                          "radius double,"             //圆半径
                                                          "startAngle int,"         //起始角度
                                                          "endAngle int)");     //终止角度
        /*塔吊群信息*/
        /*
        QString craneName;            //塔吊名称(16字节)
        quint8 craneID;                    //塔机ID
        quint8 groupID;                    //塔群ID
        quint8 firmID;                      //厂商ID
        //实时数据信息
        double angle;                        //角度
        double span;                         //变幅
        double hookHeight;                //吊钩高度
        double rotateSpeed;            //转速
        double wight;                           //吊重
        //固定信息
        double coordinateX;             //X坐标(-3276.8-3276.8)
        double coordinateY;             //Y坐标(-3276.8-3276.8)
        double craneHeight;
        double craneTopHeight;      // 塔顶高度(0-25.6)
        double forearmLen;             //前臂长(0-6553.5)
        double backarmLen;            //后臂长(0-6553.5)
        QString craneMAC;                      //塔机IMEI
        //绘图信息
        QPoint drawCenter;                  //绘图圆心
        //qint32 DrawCircleX;                 //绘图圆心X
       // qint32 DrawCircleY;                 //绘图圆形Y
        qint32 drawForeLen;                //绘图前臂长
        qint32 drawBackLen;               //绘图后臂长
        qint32 drawSpan;                    //绘图变幅
        QColor paintColor;                  //绘图颜色
        */
        /*
                                                          "angle double,"               //角度
                                                          "span double,"                //变幅
                                                          "hookHeight double,"      //吊钩高度
                                                          "rotateSpeed double,"      //转速
                                                          "wight double,"               //吊重
 */
        q.exec("create table CranesData(id integer primary key ,"
                                                          "craneName varchar(16),"       //1塔吊名称
                                                          "craneMAC varchar(16),"           //2塔吊IMEI(MAC)
                                                          "craneID int,"    //3塔吊ID
                                                          "groupID int,"        //4塔群ID
                                                          "firmID int,"             //5厂商ID
                                                          "angle double,"
                                                          "span double,"
                                                          "hookHeight double,"
                                                           "windspeed double,"
                                                          "rotateSpeed double,"

                                                          "coordinateX double,"      //6 X坐标(-3276.8-3276.8)
                                                          "coordinateY double,"      //7 Y坐标(-3276.8-3276.8)
                                                          "craneHeight double,"      //8 塔吊高
                                                          "craneTopHeight double,"      //9 塔吊顶高

                                                          "forearmLen double,"           //10 前臂长(0-6553.5)
                                                          "backarmLen double,"           //11 后臂长(0-6553.5)

                                                          "DrawCircleX int,"                      //12 圆X
                                                          "DrawCircleY int,"                      //13 圆Y
                                                          "drawForeLen int,"                         //14 绘图前臂长
                                                          "drawBackLen int,"                         //15 绘图后臂长
                                                          "drawSpan int)");                           //16 绘图变幅

        q.exec("create table LimitData(id integer primary key ,"
                                                          "limitVersion int,"        //限位信息版本
                                                          "leftLimit double,"          //左限位(-3276.8-3276.8)
                                                          "rightLimit double,"    //3//右限位(-3276.8-3276.8)
                                                          "farLimit double,"        //4远限位(0-25.5)
                                                          "nearLmit double,"             //5进限位(0-25.5)

                                                          "highLimit double,"      //6 高限位(0-25.5)
                                                          "weightLimit double,"      //7 吊重限位(0-655.35)
                                                          "momentLimit double,"      //8 力矩限位值(0-655.35)
                                                          "enableBit int)");                           //传感器使能标志位
        /*  quint16 craneKDVersion;      //信息版本
          double weightK;                     //吊重K值
          double weightD;                      //吊重K值
          double spanK;                         //变幅K值
          double spanD;                         //变幅D值
          double heightK;                        //高度K值
          double heightD;                        //高度D值
          QString serverIP;                       //IP
          QString serverPort;                    //端口
          QString ZigbeeChannel;             //通信信道
          QString ZigbeeID;                      //网络ID*/
          q.exec("create table KdData(id integer primary key ,"
                                                            "weightK double,"        //1吊重K值
                                                            "weightD double,"          //2吊重D值
                                                            "spanK double,"               //3变幅K值
                                                            "spanD double,"                //4变幅D值
                                                            "heightK double,"             //5高度K值
                                                            "heightD double,"             //6高度D值
                                                            "serverIP varchar(16),"                //7IP
                                                            "serverPort varchar,"             //8端口
                                                            "ZigbeeChannel varchar,"      //9 通信信道
                                                            "ZigbeeID varchar)");             //10网络ID

          q.exec("create table FingerStore(id integer primary key ,"
                                                          "DriverID int,"
                                                          "DriverVersion int)");
          //添加了FingerStore(DriverID,司机ID,0-9
          //DriverVersion)版本
          //信息 13.11.18
        q.exec();
        }
}
Ejemplo n.º 7
0
Quaternion Quaternion::getInversed() const
{
    Quaternion q(*this);
    q.inverse();
    return q;
}
Ejemplo n.º 8
0
void ComputeCorrelatedError(Int_t npoints, // number of measurements
			    Double_t val[], // the individual measurements
			    Double_t sigma_stat[], // statistical error
			    Double_t sigma_sys_uncorr[], // uncorrelated systematic error
			    Double_t sigma_sys_corr[], // correlated systematic error
			    Double_t & mean, // returned: the mean value
			    Double_t & sigma) // returned: the sigma
{
  // build covariance matrix
  TMatrix V(npoints, npoints);
  Double_t temp;
  for (Int_t i = 0; i < npoints; i++) {
    // diagonal
    V(i,i) = q(sigma_stat[i])+q(sigma_sys_uncorr[i])+q(sigma_sys_corr[i]);
    // off-diagonal
    for (Int_t j = i+1; j < npoints; j++) {
      temp = q(sigma_sys_corr[i]);
      V(i,j) = temp;
      V(j,i) = temp;
    }
  }

  if (gLogLevel > 10)
    V.Print();

  // invert covariance matrix
  TMatrix IV(TMatrix::kInverted, V);

  if (gLogLevel > 10)
    IV.Print();

  // compute weight vector
  TVector w(npoints);
  Double_t ivsum = 0;  // sum of all matrix elements
  Double_t rsum;       // sum of a row
  for (Int_t i = 0; i < npoints; i++) {
    rsum = 0;
    for (Int_t j = 0; j < npoints; j++) {
       rsum += IV(i,j);
    }
    w(i) = rsum;
    ivsum += rsum;
    if (gLogLevel > 10)
      printf("rsum = %8.4f,  ivsum = %8.4f\n", rsum, ivsum);
  }

  // normalize elements of weight vector
  Double_t wsum = 0;
  for (Int_t i = 0; i < npoints; i++) {
    w(i) /= ivsum;
    wsum += w(i);
  }

  if (gLogLevel > 10) {
    w.Print();
    printf("wsum = %8.4f\n", wsum);
  }

  // compute averaged value
  mean = 0;
  for (Int_t i = 0; i < npoints; i++) {
    mean += w(i) * val[i];
  }
  if (gLogLevel > 2)
    printf("mean: %8.4f\n", mean);

  // compute variance
  Double_t variance = 0;
  for (Int_t i = 0; i < npoints; i++) {
    for (Int_t j = 0; j < npoints; j++) {
      variance += w(i) * V(i,j) * w(j);
    }
  }
  sigma = TMath::Sqrt(variance);
  if (gLogLevel > 2)
    printf("variance: %8.4f, sigma = %8.4f\n", variance, sigma);
}
#define g    f
#define z    z[0]
#define h    g(~
#define m(a) a(w)
#define w    0,1
#define t(a) a
#define p()    int
#define q(x)   x
#define r(x,y) x ## y
#define str(x) #x
#define A (B)
#define B (C)
#define C (A)

#pragma start
f(y+1) + f(f(z)) % t(t(g)(0) + t)(1);
#pragma compare "f(2*(y+1))+f(2*(f(2*(z[0]))))%f(2*(0))+t(1);"
#pragma start
g(x+(3,4)-w) | h 5) & m (f)^m(m);
#pragma compare "f(2*(2+(3,4)-0,1))|f(2*(~5))&f(2*(0,1))^m(0,1);"
#pragma start
p() i[q()] = { q(1), r(2,3), r(4,), r(,5), r(,) };
#pragma compare "int i[]={1,23,4,5,};"
#pragma start
char c[2][6] = { str(hello), str() };
#pragma compare "char c[2][6]={\"hello\",\"\"};"
#pragma start
ABC=A+B+C;
#pragma compare "ABC=(((A)))+(((B)))+(((C)));"

Ejemplo n.º 10
0
void pose_esti::navCallback(const ardrone_pose_estimation::NavdataConstPtr& nav_msg)
{
   std_msgs::String str;
   navdata = *nav_msg;
   now = ros::Time::now();

   if(ptam_init_commanded && vslam_states.data =="PTAM_uninitialized")
   {
     goal_pose.z+=HEIGHT_TICK;
	 str.data = "ptam_initial";
	 ptam_init_pub.publish(str);
   }

   
   if(take_off&& !pick_takeoff_time)
   {
	   cout<<"Pick takeoff time"<<endl;
	   takeoff_start = now;
	   pick_takeoff_time=true;
	   
   	}

   if(!pick_init_yaw)
   {
	   pick_init_yaw=true;
	   init_yaw = deg2rad(nav_msg->rotZ);
	   kalman.init();
	   cout<<"init_yaw = "<<init_yaw<<endl;
   	}
	   

   double pitch,roll,vx,vy,z,vx_medi,vy_medi,vx_esti,vy_esti;

   if(init)
   {
	    //Radian
	    //yaw = deg2rad(nav_msg->rotZ)-init_yaw;
		cur_measured_yaw = deg2rad(nav_msg->rotZ);
		
		pitch = deg2rad(nav_msg->rotY);
		roll = deg2rad(nav_msg->rotX);
		vx = nav_msg->vx;
		vy = nav_msg->vy;
		z = nav_msg->altd;
		cur_pose.z = nav_msg->altd; //metre
		
		measure_vel(0) = vx;
		measure_vel(1) = vy;
		double comps_yaw;
		ros::Duration nav_ts = now-past;
		ts = nav_ts.toSec();
		//cout<<"ts = "<<ts<<endl;

	    comps_yaw=compensate_yaw(cur_measured_yaw,init_yaw);
		yaw_test=comps_yaw;
		median_vel=kalman.median(measure_vel,&median_flag);
		
		
		cur_pose.x = prev_pose.x + ts*(cos(comps_yaw)*vx-sin(comps_yaw)*vy);  //metre
		cur_pose.y = prev_pose.y + ts*(sin(comps_yaw)*vx+cos(comps_yaw)*vy);  //metre

		
		

	    if(median_flag)
		{
			vx_medi= median_vel(0);
			vy_medi= median_vel(1);

			measure << ptam_pose.pose.pose.position.x,
				       ptam_pose.pose.pose.position.y,
				       median_vel(0),
				       median_vel(1);
			esti=kalman.process(measure,comps_yaw,&invertable);
			
			if(invertable)
			{
				vx_esti=esti(2);
				vy_esti=-esti(3);
				fprintf(fp_vel,"%f,%f,%f,%f,%f,%f,%f\n",median_now.toSec(),vx,vy,esti(2),esti(3),median_vel(0),median_vel(1));
				if(vslam_states.data=="PTAM_initialized") publish_kf_transform();
				
		// Taking off to hovering takes roughly 4 sec after sending 
		// takeoff command.
		
		//if( now.toSec() - takeoff_start.toSec() > 4 && pick_takeoff_time)
		if(1)
		{
			double cmd_temp_z;
			cmd_temp_z=pid_z.updatePid(z-goal_pose.z,now-past);

					////////////////////////////////
					//     Yaw control
					////////////////////////////////


					/////////////////////////////////////////////////////
					//     Horizontal velocity and position control
					/////////////////////////////////////////////////////

					double cmd_temp_vx=0;
					double cmd_temp_x=0;
					double cmd_temp_vy=0;
					double cmd_temp_y=0;
					
					pid_pos_x.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]);  //p , i , d, i_max, i_min
					pid_pos_y.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]);  //p , i , d, i_max, i_min
					pid_vel_x.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]);  //p , i , d, i_max, i_min
					pid_vel_y.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]);  //p , i , d, i_max, i_min
					

					cmd_temp_x=pid_pos_x.updatePid(esti(0)-goal_pose.x,now-past);
					cmd_temp_y=pid_pos_y.updatePid(esti(1)-goal_pose.y,now-past);

			
			if(cmd_temp_z >= 1)
				cmd_vel.linear.z=1;
			else if (cmd_temp_z <=-1)
				cmd_vel.linear.z=-1;
			else 
				cmd_vel.linear.z  = cmd_temp_z;
			
			cmd_vel.angular.x = 0;
			cmd_vel.angular.y = 0;
			//cmd_vel.angular.z = 0;

	        //Temporaly disable x,y controllers.
			//cmd_vel.linear.x=0;
			//cmd_vel.linear.y=0;

			if(vslam_states.data=="PTAM_initialized")
			{
				if(cmd_temp_x >= 1)
					cmd_vel.linear.x=1;
				else if (cmd_temp_x <=-1)
					cmd_vel.linear.x=-1;
				else 
				cmd_vel.linear.x=cmd_temp_x;
					


				if(cmd_temp_y >= 1)
					cmd_vel.linear.y=1;
				else if (cmd_temp_y <=-1)
					cmd_vel.linear.y=-1;
				else 
				cmd_vel.linear.y=cmd_temp_y;
			}


			if(vslam_states.data=="PTAM_uninitialized")
			{
				ROS_INFO("HERE2");
				cmd_vel.linear.x=0;
				cmd_vel.linear.y=0;
				cmd_vel.angular.x = 0;
				cmd_vel.angular.y = 0;
				cmd_vel.angular.z = 0;
			}

			cmd_pub.publish(cmd_vel);
		}

		//Before taking off, only sending null commands to the drone.
		else
		{
			ROS_INFO("HERE1");
			cmd_vel.linear.x  =0;
			cmd_vel.linear.y  = 0;
			cmd_vel.angular.x = 0;
			cmd_vel.angular.y = 0;
			cmd_vel.angular.z = 0;
			cmd_pub.publish(cmd_vel);

		}
			
		//rotx(pi)
		tf::Matrix3x3 wRb(1,0,0,
						0,-1,0,
						0,0,-1);

		//
	    tf::Quaternion btq;
		tf::Matrix3x3 temp = wRb.transpose();
		temp.getRotation(btq);
			
		tf::Transform world_to_base;
		world_to_base.setIdentity();
		
		//
		tf::Quaternion q(0,0,0,0);

		//Yaw,Pitch,Roll order
		//q.setEulerZYX(comps_yaw,pitch,roll);	
		q.setEulerZYX(comps_yaw,0,0);	
		
		world_to_base.setRotation(btq*q);

		tf::Vector3 origin;
		origin.setValue (cur_pose.x,
				           cur_pose.y,
				           cur_pose.z);
		world_to_base.setOrigin (origin);

		tf::StampedTransform world_to_base_tf(world_to_base, nav_msg->header.stamp, world_frame, imu_frame);
		tf_broadcaster.sendTransform (world_to_base_tf);

		tf::Vector3 t = world_to_base.getOrigin();
		
		publish_pose(t.x(),t.y(),t.z(),roll,pitch,comps_yaw);
		publish_new_pose(esti(0),esti(1),cur_pose.z,roll,pitch,PTAM_Yaw);
		prev_pose = cur_pose;
		past = now;
			//median_past = median_now;
		past_measured_yaw = cur_measured_yaw;
			median_flag=0;
   } //if(invertable)
		} //if(median_flag)
   } //if(init)

   // At the very first time we don't know the samping time,
   // Skip one frame.
   else 
   {
	   past = now;
	   init=true;
	   past_measured_yaw = deg2rad(nav_msg->rotZ);
   }
   
   
   //ROS_INFO("navCallback");
   

   
}
Ejemplo n.º 11
0
/**
 * Calculate the points of intersection for the given detector with cuboid
 * surrounding the
 * detector position in HKL
 * @param theta Polar angle withd detector
 * @param phi Azimuthal angle with detector
 * @return A list of intersections in HKL space
 */
std::vector<Kernel::VMD> MDNormSCD::calculateIntersections(const double theta,
                                                           const double phi) {
  V3D q(-sin(theta) * cos(phi), -sin(theta) * sin(phi), 1. - cos(theta));
  q = m_rubw * q;
  if (convention == "Crystallography") {
    q *= -1;
  }

  double hStart = q.X() * m_kiMin, hEnd = q.X() * m_kiMax;
  double kStart = q.Y() * m_kiMin, kEnd = q.Y() * m_kiMax;
  double lStart = q.Z() * m_kiMin, lEnd = q.Z() * m_kiMax;

  double eps = 1e-7;

  auto hNBins = m_hX.size();
  auto kNBins = m_kX.size();
  auto lNBins = m_lX.size();
  std::vector<Kernel::VMD> intersections;
  intersections.reserve(hNBins + kNBins + lNBins + 8);

  // calculate intersections with planes perpendicular to h
  if (fabs(hStart - hEnd) > eps) {
    double fmom = (m_kiMax - m_kiMin) / (hEnd - hStart);
    double fk = (kEnd - kStart) / (hEnd - hStart);
    double fl = (lEnd - lStart) / (hEnd - hStart);
    if (!m_hIntegrated) {
      for (size_t i = 0; i < hNBins; i++) {
        double hi = m_hX[i];
        if ((hi >= m_hmin) && (hi <= m_hmax) &&
            ((hStart - hi) * (hEnd - hi) < 0)) {
          // if hi is between hStart and hEnd, then ki and li will be between
          // kStart, kEnd and lStart, lEnd and momi will be between m_kiMin and
          // KnincidemtmMax
          double ki = fk * (hi - hStart) + kStart;
          double li = fl * (hi - hStart) + lStart;
          if ((ki >= m_kmin) && (ki <= m_kmax) && (li >= m_lmin) &&
              (li <= m_lmax)) {
            double momi = fmom * (hi - hStart) + m_kiMin;
            Mantid::Kernel::VMD v(hi, ki, li, momi);
            intersections.push_back(v);
          }
        }
      }
    }

    double momhMin = fmom * (m_hmin - hStart) + m_kiMin;
    if ((momhMin > m_kiMin) && (momhMin < m_kiMax)) {
      // khmin and lhmin
      double khmin = fk * (m_hmin - hStart) + kStart;
      double lhmin = fl * (m_hmin - hStart) + lStart;
      if ((khmin >= m_kmin) && (khmin <= m_kmax) && (lhmin >= m_lmin) &&
          (lhmin <= m_lmax)) {
        Mantid::Kernel::VMD v(m_hmin, khmin, lhmin, momhMin);
        intersections.push_back(v);
      }
    }
    double momhMax = fmom * (m_hmax - hStart) + m_kiMin;
    if ((momhMax > m_kiMin) && (momhMax < m_kiMax)) {
      // khmax and lhmax
      double khmax = fk * (m_hmax - hStart) + kStart;
      double lhmax = fl * (m_hmax - hStart) + lStart;
      if ((khmax >= m_kmin) && (khmax <= m_kmax) && (lhmax >= m_lmin) &&
          (lhmax <= m_lmax)) {
        Mantid::Kernel::VMD v(m_hmax, khmax, lhmax, momhMax);
        intersections.push_back(v);
      }
    }
  }

  // calculate intersections with planes perpendicular to k
  if (fabs(kStart - kEnd) > eps) {
    double fmom = (m_kiMax - m_kiMin) / (kEnd - kStart);
    double fh = (hEnd - hStart) / (kEnd - kStart);
    double fl = (lEnd - lStart) / (kEnd - kStart);
    if (!m_kIntegrated) {
      for (size_t i = 0; i < kNBins; i++) {
        double ki = m_kX[i];
        if ((ki >= m_kmin) && (ki <= m_kmax) &&
            ((kStart - ki) * (kEnd - ki) < 0)) {
          // if ki is between kStart and kEnd, then hi and li will be between
          // hStart, hEnd and lStart, lEnd
          double hi = fh * (ki - kStart) + hStart;
          double li = fl * (ki - kStart) + lStart;
          if ((hi >= m_hmin) && (hi <= m_hmax) && (li >= m_lmin) &&
              (li <= m_lmax)) {
            double momi = fmom * (ki - kStart) + m_kiMin;
            Mantid::Kernel::VMD v(hi, ki, li, momi);
            intersections.push_back(v);
          }
        }
      }
    }

    double momkMin = fmom * (m_kmin - kStart) + m_kiMin;
    if ((momkMin > m_kiMin) && (momkMin < m_kiMax)) {
      // hkmin and lkmin
      double hkmin = fh * (m_kmin - kStart) + hStart;
      double lkmin = fl * (m_kmin - kStart) + lStart;
      if ((hkmin >= m_hmin) && (hkmin <= m_hmax) && (lkmin >= m_lmin) &&
          (lkmin <= m_lmax)) {
        Mantid::Kernel::VMD v(hkmin, m_kmin, lkmin, momkMin);
        intersections.push_back(v);
      }
    }
    double momkMax = fmom * (m_kmax - kStart) + m_kiMin;
    if ((momkMax > m_kiMin) && (momkMax < m_kiMax)) {
      // hkmax and lkmax
      double hkmax = fh * (m_kmax - kStart) + hStart;
      double lkmax = fl * (m_kmax - kStart) + lStart;
      if ((hkmax >= m_hmin) && (hkmax <= m_hmax) && (lkmax >= m_lmin) &&
          (lkmax <= m_lmax)) {
        Mantid::Kernel::VMD v(hkmax, m_kmax, lkmax, momkMax);
        intersections.push_back(v);
      }
    }
  }

  // calculate intersections with planes perpendicular to l
  if (fabs(lStart - lEnd) > eps) {
    double fmom = (m_kiMax - m_kiMin) / (lEnd - lStart);
    double fh = (hEnd - hStart) / (lEnd - lStart);
    double fk = (kEnd - kStart) / (lEnd - lStart);
    if (!m_lIntegrated) {
      for (size_t i = 0; i < lNBins; i++) {
        double li = m_lX[i];
        if ((li >= m_lmin) && (li <= m_lmax) &&
            ((lStart - li) * (lEnd - li) < 0)) {
          // if li is between lStart and lEnd, then hi and ki will be between
          // hStart, hEnd and kStart, kEnd
          double hi = fh * (li - lStart) + hStart;
          double ki = fk * (li - lStart) + kStart;
          if ((hi >= m_hmin) && (hi <= m_hmax) && (ki >= m_kmin) &&
              (ki <= m_kmax)) {
            double momi = fmom * (li - lStart) + m_kiMin;
            Mantid::Kernel::VMD v(hi, ki, li, momi);
            intersections.push_back(v);
          }
        }
      }
    }

    double momlMin = fmom * (m_lmin - lStart) + m_kiMin;
    if ((momlMin > m_kiMin) && (momlMin < m_kiMax)) {
      // hlmin and klmin
      double hlmin = fh * (m_lmin - lStart) + hStart;
      double klmin = fk * (m_lmin - lStart) + kStart;
      if ((hlmin >= m_hmin) && (hlmin <= m_hmax) && (klmin >= m_kmin) &&
          (klmin <= m_kmax)) {
        Mantid::Kernel::VMD v(hlmin, klmin, m_lmin, momlMin);
        intersections.push_back(v);
      }
    }
    double momlMax = fmom * (m_lmax - lStart) + m_kiMin;
    if ((momlMax > m_kiMin) && (momlMax < m_kiMax)) {
      // khmax and lhmax
      double hlmax = fh * (m_lmax - lStart) + hStart;
      double klmax = fk * (m_lmax - lStart) + kStart;
      if ((hlmax >= m_hmin) && (hlmax <= m_hmax) && (klmax >= m_kmin) &&
          (klmax <= m_kmax)) {
        Mantid::Kernel::VMD v(hlmax, klmax, m_lmax, momlMax);
        intersections.push_back(v);
      }
    }
  }

  // add endpoints
  if ((hStart >= m_hmin) && (hStart <= m_hmax) && (kStart >= m_kmin) &&
      (kStart <= m_kmax) && (lStart >= m_lmin) && (lStart <= m_lmax)) {
    Mantid::Kernel::VMD v(hStart, kStart, lStart, m_kiMin);
    intersections.push_back(v);
  }
  if ((hEnd >= m_hmin) && (hEnd <= m_hmax) && (kEnd >= m_kmin) &&
      (kEnd <= m_kmax) && (lEnd >= m_lmin) && (lEnd <= m_lmax)) {
    Mantid::Kernel::VMD v(hEnd, kEnd, lEnd, m_kiMax);
    intersections.push_back(v);
  }

  // sort intersections by momentum
  typedef std::vector<Mantid::Kernel::VMD>::iterator IterType;
  std::stable_sort<IterType, bool (*)(const Mantid::Kernel::VMD &,
                                      const Mantid::Kernel::VMD &)>(
      intersections.begin(), intersections.end(), compareMomentum);

  return intersections;
}
Ejemplo n.º 12
0
extern "C" magma_int_t
magma_dbpcg(
    magma_d_matrix A, magma_d_matrix b, magma_d_matrix *x,
    magma_d_solver_par *solver_par,
    magma_d_preconditioner *precond_par,
    magma_queue_t queue )
{
    magma_int_t info = 0;
    
    magma_int_t i, num_vecs = b.num_rows/A.num_rows;

    // prepare solver feedback
    solver_par->solver = Magma_PCG;
    solver_par->numiter = 0;
    solver_par->spmv_count = 0;
    solver_par->info = MAGMA_SUCCESS;

    // local variables
    double c_zero = MAGMA_D_ZERO, c_one = MAGMA_D_ONE;
    
    magma_int_t dofs = A.num_rows;

    // GPU workspace
    magma_d_matrix r={Magma_CSR}, rt={Magma_CSR}, p={Magma_CSR}, q={Magma_CSR}, h={Magma_CSR};

    
    // solver variables
    double *alpha={0}, *beta={0};
    alpha = NULL;
    beta = NULL;


    double *nom={0}, *nom0={0}, *r0={0}, *gammaold={0}, *gammanew={0}, *den={0}, *res={0}, *residual={0};
    nom        = NULL;
    nom0       = NULL;
    r0         = NULL;
    gammaold   = NULL;
    gammanew   = NULL;
    den        = NULL;
    res        = NULL;
    residual   = NULL;
    
    CHECK( magma_dmalloc_cpu(&alpha, num_vecs));
    CHECK( magma_dmalloc_cpu(&beta, num_vecs));
    CHECK( magma_dmalloc_cpu(&residual, num_vecs));
    CHECK( magma_dmalloc_cpu(&nom, num_vecs));
    CHECK( magma_dmalloc_cpu(&nom0, num_vecs));
    CHECK( magma_dmalloc_cpu(&r0, num_vecs));
    CHECK( magma_dmalloc_cpu(&gammaold, num_vecs));
    CHECK( magma_dmalloc_cpu(&gammanew, num_vecs));
    CHECK( magma_dmalloc_cpu(&den, num_vecs));
    CHECK( magma_dmalloc_cpu(&res, num_vecs));
    CHECK( magma_dmalloc_cpu(&residual, num_vecs));
    
    CHECK( magma_dvinit( &r, Magma_DEV, dofs*num_vecs, 1, c_zero, queue ));
    CHECK( magma_dvinit( &rt, Magma_DEV, dofs*num_vecs, 1, c_zero, queue ));
    CHECK( magma_dvinit( &p, Magma_DEV, dofs*num_vecs, 1, c_zero, queue ));
    CHECK( magma_dvinit( &q, Magma_DEV, dofs*num_vecs, 1, c_zero, queue ));
    CHECK( magma_dvinit( &h, Magma_DEV, dofs*num_vecs, 1, c_zero, queue ));

    // solver setup
    CHECK(  magma_dresidualvec( A, b, *x, &r, nom0, queue));

    // preconditioner
    CHECK( magma_d_applyprecond_left( MagmaNoTrans, A, r, &rt, precond_par, queue ));
    CHECK( magma_d_applyprecond_right( MagmaNoTrans, A, rt, &h, precond_par, queue ));

    magma_dcopy( dofs*num_vecs, h.dval, 1, p.dval, 1, queue );                 // p = h

    for( i=0; i<num_vecs; i++) {
        nom[i] = MAGMA_D_REAL( magma_ddot( dofs, r(i), 1, h(i), 1, queue ) );
        nom0[i] = magma_dnrm2( dofs, r(i), 1, queue );
    }
                                          
    CHECK( magma_d_spmv( c_one, A, p, c_zero, q, queue ));             // q = A p

    for( i=0; i<num_vecs; i++)
        den[i] = MAGMA_D_REAL( magma_ddot( dofs, p(i), 1, q(i), 1, queue ) );  // den = p dot q

    solver_par->init_res = nom0[0];
    
    if ( (r0[0] = nom[0] * solver_par->rtol) < ATOLERANCE )
        r0[0] = ATOLERANCE;
    // check positive definite
    if (den[0] <= 0.0) {
        printf("Operator A is not postive definite. (Ar,r) = %f\n", den[0]);
        info = MAGMA_NONSPD; 
        goto cleanup;
    }
    if ( nom[0] < r0[0] ) {
        solver_par->final_res = solver_par->init_res;
        solver_par->iter_res = solver_par->init_res;
        goto cleanup;
    }

    //Chronometry
    real_Double_t tempo1, tempo2;
    tempo1 = magma_sync_wtime( queue );
    if ( solver_par->verbose > 0 ) {
        solver_par->res_vec[0] = (real_Double_t)nom0[0];
        solver_par->timing[0] = 0.0;
    }
    
    solver_par->numiter = 0;
    solver_par->spmv_count = 0;
    // start iteration
    do
    {
        solver_par->numiter++;
        // preconditioner
        CHECK( magma_d_applyprecond_left( MagmaNoTrans, A, r, &rt, precond_par, queue ));
        CHECK( magma_d_applyprecond_right( MagmaNoTrans, A, rt, &h, precond_par, queue ));


        for( i=0; i<num_vecs; i++)
            gammanew[i] = MAGMA_D_REAL( magma_ddot( dofs, r(i), 1, h(i), 1, queue ) );  // gn = < r,h>


        if ( solver_par->numiter==1 ) {
            magma_dcopy( dofs*num_vecs, h.dval, 1, p.dval, 1, queue );                    // p = h
        } else {
            for( i=0; i<num_vecs; i++) {
                beta[i] = MAGMA_D_MAKE(gammanew[i]/gammaold[i], 0.);       // beta = gn/go
                magma_dscal( dofs, beta[i], p(i), 1, queue );            // p = beta*p
                magma_daxpy( dofs, c_one, h(i), 1, p(i), 1, queue ); // p = p + h
            }
        }

        CHECK( magma_d_spmv( c_one, A, p, c_zero, q, queue ));   // q = A p
        solver_par->spmv_count++;
     //   magma_d_bspmv_tuned( dofs, num_vecs, c_one, A, p.dval, c_zero, q.dval, queue );


        for( i=0; i<num_vecs; i++) {
            den[i] = MAGMA_D_REAL(magma_ddot( dofs, p(i), 1, q(i), 1, queue) );
                // den = p dot q

            alpha[i] = MAGMA_D_MAKE(gammanew[i]/den[i], 0.);
            magma_daxpy( dofs,  alpha[i], p(i), 1, x->dval+dofs*i, 1, queue ); // x = x + alpha p
            magma_daxpy( dofs, -alpha[i], q(i), 1, r(i), 1, queue );      // r = r - alpha q
            gammaold[i] = gammanew[i];

            res[i] = magma_dnrm2( dofs, r(i), 1, queue );
        }

        if ( solver_par->verbose > 0 ) {
            tempo2 = magma_sync_wtime( queue );
            if ( (solver_par->numiter)%solver_par->verbose==0 ) {
                solver_par->res_vec[(solver_par->numiter)/solver_par->verbose]
                        = (real_Double_t) res[0];
                solver_par->timing[(solver_par->numiter)/solver_par->verbose]
                        = (real_Double_t) tempo2-tempo1;
            }
        }


        if (  res[0]/nom0[0]  < solver_par->rtol ) {
            break;
        }
    }
    while ( solver_par->numiter+1 <= solver_par->maxiter );
    
    tempo2 = magma_sync_wtime( queue );
    solver_par->runtime = (real_Double_t) tempo2-tempo1;
    CHECK( magma_dresidual( A, b, *x, residual, queue ));
    solver_par->iter_res = res[0];
    solver_par->final_res = residual[0];

    if ( solver_par->numiter < solver_par->maxiter ) {
        solver_par->info = MAGMA_SUCCESS;
    } else if ( solver_par->init_res > solver_par->final_res ) {
        if ( solver_par->verbose > 0 ) {
            if ( (solver_par->numiter)%solver_par->verbose==0 ) {
                solver_par->res_vec[(solver_par->numiter)/solver_par->verbose]
                        = (real_Double_t) res[0];
                solver_par->timing[(solver_par->numiter)/solver_par->verbose]
                        = (real_Double_t) tempo2-tempo1;
            }
        }
        info = MAGMA_SLOW_CONVERGENCE;
        if( solver_par->iter_res < solver_par->rtol*solver_par->init_res ){
            info = MAGMA_SUCCESS;
        }
    }
    else {
        if ( solver_par->verbose > 0 ) {
            if ( (solver_par->numiter)%solver_par->verbose==0 ) {
                solver_par->res_vec[(solver_par->numiter)/solver_par->verbose]
                        = (real_Double_t) res[0];
                solver_par->timing[(solver_par->numiter)/solver_par->verbose]
                        = (real_Double_t) tempo2-tempo1;
            }
        }
        info = MAGMA_DIVERGENCE;
    }
    for( i=0; i<num_vecs; i++) {
        printf("%.4e  ",res[i]);
    }
    printf("\n");
    for( i=0; i<num_vecs; i++) {
        printf("%.4e  ",residual[i]);
    }
    printf("\n");

cleanup:
    magma_dmfree(&r, queue );
    magma_dmfree(&rt, queue );
    magma_dmfree(&p, queue );
    magma_dmfree(&q, queue );
    magma_dmfree(&h, queue );

    magma_free_cpu(alpha);
    magma_free_cpu(beta);
    magma_free_cpu(nom);
    magma_free_cpu(nom0);
    magma_free_cpu(r0);
    magma_free_cpu(gammaold);
    magma_free_cpu(gammanew);
    magma_free_cpu(den);
    magma_free_cpu(res);

    solver_par->info = info;
    return info;
}   /* magma_dbpcg */
Ejemplo n.º 13
0
void MagneticModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
{
  State::ConstOrientationType q(state.getOrientation());

  if (state.getOrientationIndex() >= 0) {
    const double& m1 = magnetic_field_reference_.x();
    const double& m2 = magnetic_field_reference_.y();
//    const double& m3 = magnetic_field_reference_.y();

//    C_full_(0,State::QUATERNION_W) =  2.0*q.w() * magnetic_field_reference_.x() + 2.0*q.z() * magnetic_field_reference_.y() - 2.0*q.y() * magnetic_field_reference_.z();
//    C_full_(0,State::QUATERNION_X) =  2.0*q.x() * magnetic_field_reference_.x() + 2.0*q.y() * magnetic_field_reference_.y() + 2.0*q.z() * magnetic_field_reference_.z();
//    C_full_(0,State::QUATERNION_Y) = -2.0*q.y() * magnetic_field_reference_.x() + 2.0*q.x() * magnetic_field_reference_.y() - 2.0*q.w() * magnetic_field_reference_.z();
//    C_full_(0,State::QUATERNION_Z) = -2.0*q.z() * magnetic_field_reference_.x() + 2.0*q.w() * magnetic_field_reference_.y() + 2.0*q.x() * magnetic_field_reference_.z();
//    C_full_(1,State::QUATERNION_W) = -2.0*q.z() * magnetic_field_reference_.x() + 2.0*q.w() * magnetic_field_reference_.y() + 2.0*q.x() * magnetic_field_reference_.z();
//    C_full_(1,State::QUATERNION_X) =  2.0*q.y() * magnetic_field_reference_.x() - 2.0*q.x() * magnetic_field_reference_.y() + 2.0*q.w() * magnetic_field_reference_.z();
//    C_full_(1,State::QUATERNION_Y) =  2.0*q.x() * magnetic_field_reference_.x() + 2.0*q.y() * magnetic_field_reference_.y() + 2.0*q.z() * magnetic_field_reference_.z();
//    C_full_(1,State::QUATERNION_Z) = -2.0*q.w() * magnetic_field_reference_.x() - 2.0*q.z() * magnetic_field_reference_.y() + 2.0*q.y() * magnetic_field_reference_.z();
//    C_full_(2,State::QUATERNION_W) =  2.0*q.y() * magnetic_field_reference_.x() - 2.0*q.x() * magnetic_field_reference_.y() + 2.0*q.w() * magnetic_field_reference_.z();
//    C_full_(2,State::QUATERNION_X) =  2.0*q.z() * magnetic_field_reference_.x() - 2.0*q.w() * magnetic_field_reference_.y() - 2.0*q.x() * magnetic_field_reference_.z();
//    C_full_(2,State::QUATERNION_Y) =  2.0*q.w() * magnetic_field_reference_.x() + 2.0*q.z() * magnetic_field_reference_.y() - 2.0*q.y() * magnetic_field_reference_.z();
//    C_full_(2,State::QUATERNION_Z) =  2.0*q.x() * magnetic_field_reference_.x() + 2.0*q.y() * magnetic_field_reference_.y() + 2.0*q.z() * magnetic_field_reference_.z();

    // return C_full_;

    // q = [qw qx qy qz]';
    // dq/dyaw * dyaw*dq = 1/2 * [-qz -qy qx qw]' * 2 * [-qz; -qy; qx; qw] =
    //  [ qz*qz  qz*qy -qz*qx -qz*qw ;
    //    qy*qz  qy*qy -qy*qx -qy*qw ;
    //   -qx*qz -qx*qy  qx*qx  qx*qw ;
    //   -qw*qz -qw*qy  qw*qx  qw*qw ]

//    for(int i = 0; i <= 2; ++i) {
//      C(i,State::QUATERNION_W) =  C_full_(i,State::QUATERNION_W) * q.z()*q.z() + C_full_(i,State::QUATERNION_X) * q.y()*q.z() - C_full_(i,State::QUATERNION_Y) * q.x()*q.z() - C_full_(i,State::QUATERNION_Z) * q.w()*q.z();
//      C(i,State::QUATERNION_X) =  C_full_(i,State::QUATERNION_W) * q.z()*q.y() + C_full_(i,State::QUATERNION_X) * q.y()*q.y() - C_full_(i,State::QUATERNION_Y) * q.x()*q.y() - C_full_(i,State::QUATERNION_Z) * q.w()*q.y();
//      C(i,State::QUATERNION_Y) = -C_full_(i,State::QUATERNION_W) * q.z()*q.x() - C_full_(i,State::QUATERNION_X) * q.y()*q.x() + C_full_(i,State::QUATERNION_Y) * q.x()*q.x() + C_full_(i,State::QUATERNION_Z) * q.w()*q.x();
//      C(i,State::QUATERNION_Z) = -C_full_(i,State::QUATERNION_W) * q.z()*q.w() - C_full_(i,State::QUATERNION_X) * q.y()*q.w() + C_full_(i,State::QUATERNION_Y) * q.x()*q.w() + C_full_(i,State::QUATERNION_Z) * q.w()*q.w();
//    }

//
// Simplified with symbolic Matlab toolbox:
//
// C = [  2*qz*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz),
//        2*qy*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz),
//       -2*qx*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz),
//       -2*qw*(- m2*qx^2 + 2*m1*qx*qy + m2*qz*qx + m2*qy^2 + m2*qw*qy + 2*m1*qw*qz);
//        2*qz*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2),
//        2*qy*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2),
//       -2*qx*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2),
//       -2*qw*(m1*qw^2 + 2*m2*qw*qz - m1*qx^2 - 2*m2*qx*qy + m1*qy^2 - m1*qz^2);
//       -4*qz*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz),
//       -4*qy*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz),
//        4*qx*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz),
//        4*qw*(m1*qw*qx + m2*qw*qy + m2*qx*qz - m1*qy*qz) ]

  double temp1 = -m2*q.x()*q.x() + 2*m1*q.x()*q.y() + m2*q.z()*q.x() + m2*q.y()*q.y()   + m2*q.w()*q.y() + 2*m1*q.w()*q.z();
  double temp2 =  m1*q.w()*q.w() + 2*m2*q.w()*q.z() - m1*q.x()*q.x() - 2*m2*q.x()*q.y() + m1*q.y()*q.y() - m1*q.z()*q.z();
  double temp3 =  m1*q.w()*q.x() +   m2*q.w()*q.y() + m2*q.x()*q.z() -   m1*q.y()*q.z();
  C(0,State::QUATERNION_W) =  2*q.z()*temp1;
  C(0,State::QUATERNION_X) =  2*q.y()*temp1;
  C(0,State::QUATERNION_Y) = -2*q.x()*temp1;
  C(0,State::QUATERNION_Z) = -2*q.w()*temp1;
  C(1,State::QUATERNION_W) =  2*q.z()*temp2;
  C(1,State::QUATERNION_X) =  2*q.y()*temp2;
  C(1,State::QUATERNION_Y) = -2*q.x()*temp2;
  C(1,State::QUATERNION_Z) = -2*q.w()*temp2;
  C(2,State::QUATERNION_W) = -4*q.z()*temp3;
  C(2,State::QUATERNION_X) = -4*q.y()*temp3;
  C(2,State::QUATERNION_Y) =  4*q.x()*temp3;
  C(2,State::QUATERNION_Z) =  4*q.w()*temp3;
  }
}
Ejemplo n.º 14
0
// A "wibbly" is a sphere-man with his center of mass brought low inside the body so he wobbles easily.
// Note that we do not share shapes. We could share the spheres and boxes used since they're of fixed size
// but we create new ones each when time using this method.
hkpRigidBody* CompoundBodiesDemo::createWibbly(hkVector4& position, hkPseudoRandomGenerator* generator)
{
	// We build a wibbly from 4 bodies:
	// 1. A big sphere
	// 2,3 Two arms
	// 4 A head

		// These parameters specify the wibbly size. The main (body) sphere has the radius defined
		// below. The arms have size 'boxSize'.
	hkReal radius = 1.0f;
	hkVector4 boxSize( 1.0f, 0.5f, 0.5f);
	hkReal mass = 10.0f;


	//
	// Create the shapes (we could share these between wibblies of the same size, but we don't here)
	//

	hkpSphereShape* sphere = new hkpSphereShape(radius);

	hkpSphereShape* sphere2 = new hkpSphereShape(radius * 0.3f);

	hkVector4 halfExtents(boxSize(0) * 0.5f, boxSize(1) * 0.5f, boxSize(2) * 0.5f);

	hkpBoxShape* cube = new hkpBoxShape(halfExtents, 0 );	

	//
	// Create a rigid body construction template and start filling it out
	//
	hkpRigidBodyCinfo compoundInfo;


	// We'll basically have to create a 'List' Shape  (ie. a hkpListShape) in order to have many
	// shapes in the same body. Each element of the list will be a (transformed) hkpShape, ie.
	// a hkpTransformShape (which basically is a (geometry,transformation) pair).
	// The hkpListShape constructor needs a pointer to an array of hkShapes, so we create an array here, and push
	// back the hkTransformShapes as we create them.
	hkInplaceArray<hkpShape*,4> shapeArray;

	// Create body
	{
		sphere->addReference();
		shapeArray.pushBack(sphere);
	}

	// Create head
	{
		hkVector4 offset(0.0f, 1.1f, 0.0f);
		hkpConvexTranslateShape* sphere2Trans = new hkpConvexTranslateShape( sphere2, offset );
		shapeArray.pushBack(sphere2Trans);
	}


	// Create right arm
	{

		hkTransform t;
		hkVector4 v(0.0f,0.0f,1.0f);
		hkQuaternion r(v, 0.4f);
		t.setRotation(r);
		t.getTranslation().set(0.9f, .7f, 0.0f);
		hkpConvexTransformShape* cubeTrans = new hkpConvexTransformShape( cube,t );
		shapeArray.pushBack(cubeTrans);
	}


	// Create left arm
	{
		hkTransform t;
		hkVector4 v(0.0f,0.0f,1.0f);
		hkQuaternion r(v, -0.4f);
		t.setRotation(r);
		t.getTranslation().set(-0.9f, .7f, 0.0f);
		hkpConvexTransformShape* cubeTrans = new hkpConvexTransformShape( cube,t );
		shapeArray.pushBack(cubeTrans);
	}



	// Now we can create the compound body as a hkpListShape

	hkpListShape* listShape;
	{
		listShape = new hkpListShape(shapeArray.begin(), shapeArray.getSize());
		sphere->removeReference();
		sphere2->removeReference();
		cube->removeReference();
		for (int i = 0; i < shapeArray.getSize(); ++i)
		{
			shapeArray[i]->removeReference();
		}
	}
	compoundInfo.m_shape = listShape;


	//
	// Create the rigid body
	//

	compoundInfo.m_mass = mass;
	
		// Fake an inertia tensor using a cube of side 'radius'
	hkVector4 halfCube(radius *0.5f, radius *0.5f, radius *0.5f);
	hkpMassProperties massProperties;
	hkpInertiaTensorComputer::computeBoxVolumeMassProperties(halfCube, mass, massProperties);
	compoundInfo.m_inertiaTensor = massProperties.m_inertiaTensor;

	// Now (and here's the wibbly bit) set the center of mass to be near the bottom of the
	// body sphere.
	compoundInfo.m_centerOfMass.set(0.0f,-0.8f,0.0f);
	compoundInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

	compoundInfo.m_position = position;
		// Use "random" initial orientation
	hkVector4 axis(generator->getRandRange(-1.0f,1.0f),generator->getRandRange(-1.0f,1.0f),generator->getRandRange(-1.0f,1.0f));
	axis.normalize3();
	hkQuaternion q(axis, generator->getRandRange(0,HK_REAL_PI));
	compoundInfo.m_rotation = q;


		// Finally create the body
	hkpRigidBody* compoundRigidBody = new hkpRigidBody(compoundInfo);

	listShape->removeReference();

	return compoundRigidBody;

}
Ejemplo n.º 15
0
int LibModelFile::init(const std::string &filename) {
  assert(m_initialised == false);

  std::string object;
  if (m_config.readFromFile(filename)) {
    if (m_config.findItem(SECTION_model, KEY_filename)) {
      object = (std::string)m_config.getItem(SECTION_model, KEY_filename);
    } else {
      fprintf(stderr, "[LibModelFile] Error: No md3 filename specified.\n");
      return 1;
    }
  } else {
    fprintf(stderr, "[LibModelFile] Error reading %s as varconf file. Trying as .md3 file.\n",
            filename.c_str());
    object = filename;
  }
  // Initialise transform matrix
  float matrix[4][4];
  for (int j = 0; j < 4; ++j) {
    for (int i = 0; i < 4; ++i) {
      if (i == j) matrix[j][i] = 1.0f;
      else matrix[j][i] = 0.0f;
    }
  }
  if (m_config.findItem(SECTION_model, KEY_rotation)) {
    const std::string &str=(std::string)m_config.getItem(SECTION_model, KEY_rotation);
    float w,x,y,z;
    sscanf(str.c_str(), "%f;%f;%f;%f", &w, &x, &y, &z);
    WFMath::Quaternion q(w,x,y,z);
    QuatToMatrix(q, matrix);
  }
  if (m_config.findItem(SECTION_model, KEY_scale)) {
    double s = (double)m_config.getItem(SECTION_model, KEY_scale);
    for (int i = 0; i < 4; ++i) matrix[i][i] *= s;
  }

  System::instance()->getFileHandler()->getFilePath(object);

  // Load md3 file
//  if (debug) printf("[LibModelFile] Loading: %s\n", object.c_str());

  libmd3_file *modelFile = libmd3_file_load(object.c_str());
  if (!modelFile) {
    fprintf(stderr, "[LibModelFile] Error loading %s file\n", object.c_str());
    return 1;
  }

  for (int i = 0; i < modelFile->header->mesh_count; ++i) {
    libmd3_unpack_normals(&modelFile->meshes[i]);
  }

  // Get mesh data
  libmd3_mesh *meshp = modelFile->meshes;
  for (int i = 0; i < modelFile->header->mesh_count; ++i, ++meshp) {
    StaticObject* so = new StaticObject();
    so->init();

    // Get Texture data from Mesh
    int texture_id = NO_TEXTURE_ID;
    int texture_mask_id = NO_TEXTURE_ID;
    if (meshp->mesh_header->skin_count != 0) {
      std::string name = (const char*)(meshp->skins[0].name);
      m_config.clean(name);

      // Check for texture name overrides in vconf file
      // Backwards compatibility.
      if (m_config.findItem(name, KEY_filename)) {
        name = (std::string)m_config.getItem(name, KEY_filename);
      }
      // Check for texture name overrides in vconf file
      // New method
      if (m_config.findItem(name, KEY_texture_map_0)) {
        name = (std::string)m_config.getItem(name, KEY_texture_map_0);
      }
      // Request Texture ID
      texture_id = RenderSystem::getInstance().requestTexture(name);
      texture_mask_id = RenderSystem::getInstance().requestTexture(name, true);

      float m[4];
      if (m_config.findItem(name, KEY_ambient)) {
        const std::string &str = (std::string)m_config.getItem(name, KEY_ambient);
        sscanf(str.c_str(), "%f;%f;%f;%f", &m[0], &m[1], &m[2], &m[3]);
        so->setAmbient(m); 
      }
      if (m_config.findItem(name, KEY_diffuse)) {
        const std::string &str = (std::string)m_config.getItem(name, KEY_diffuse);
        sscanf(str.c_str(), "%f;%f;%f;%f", &m[0], &m[1], &m[2], &m[3]);
        so->setDiffuse(m); 
      }
      if (m_config.findItem(name, KEY_specular)) {
        const std::string &str = (std::string)m_config.getItem(name, KEY_specular);
        sscanf(str.c_str(), "%f;%f;%f;%f", &m[0], &m[1], &m[2], &m[3]);
        so->setSpecular(m); 
      }
      if (m_config.findItem(name, KEY_emission)) {
        const std::string &str = (std::string)m_config.getItem(name, KEY_emission);
        sscanf(str.c_str(), "%f;%f;%f;%f", &m[0], &m[1], &m[2], &m[3]);
        so->setEmission(m); 
      }
      if (m_config.findItem(name, KEY_shininess)) {
        so->setShininess((double)m_config.getItem(name, KEY_shininess));
      }
      
    }
    // Set the transform
    so->getMatrix().setMatrix(matrix);
    // Set the textures
    so->setTexture(0, texture_id, texture_mask_id);
    so->setNumPoints(meshp->mesh_header->vertex_count);
    so->setNumFaces(meshp->mesh_header->triangle_count);

    // Copy data into array.
    so->createVertexData(meshp->mesh_header->vertex_count * 3);
    float *ptr = so->getVertexDataPtr();
    for (int i = 0; i < meshp->mesh_header->vertex_count * 3; ++i) {
      ptr[i] = default_scale * (float)meshp->vertices[i];
    }  
 
    so->copyTextureData(meshp->texcoords, meshp->mesh_header->vertex_count * 2);
    so->copyNormalData(meshp->normals, meshp->mesh_header->vertex_count * 3);

    so->copyIndices(meshp->triangles, meshp->mesh_header->triangle_count * 3);

    m_static_objects.push_back(so);
  }

  libmd3_file_free(modelFile);


  bool ignore_minus_z = false;

  Scaling scale = SCALE_NONE;
  Alignment align = ALIGN_NONE;
  bool process_model = false;

  if (m_config.findItem(SECTION_model, KEY_ignore_minus_z)) {
    if ((bool)m_config.getItem(SECTION_model, KEY_ignore_minus_z)) {
      process_model = true;
      ignore_minus_z = true;
    }
  }
  if (m_config.findItem(SECTION_model, KEY_z_align)) {
    if ((bool)m_config.getItem(SECTION_model, KEY_z_align)) {
      process_model = true;
      align = ALIGN_Z;
    }
  }
  if (m_config.findItem(SECTION_model, KEY_scale_isotropic)) {
    if ((bool)m_config.getItem(SECTION_model, KEY_scale_isotropic)) {
      process_model = true;
      scale = SCALE_ISOTROPIC;
    }
  }
  else if (m_config.findItem(SECTION_model, KEY_scale_isotropic_x)) {
    if ((bool)m_config.getItem(SECTION_model, KEY_scale_isotropic_x)) {
      process_model = true;
      scale = SCALE_ISOTROPIC_Z;
    }
  }
  else if (m_config.findItem(SECTION_model, KEY_scale_isotropic_y)) {
    if ((bool)m_config.getItem(SECTION_model, KEY_scale_isotropic_y)) {
      process_model = true;
      scale = SCALE_ISOTROPIC_Y;
    }
  }
  else if (m_config.findItem(SECTION_model, KEY_scale_isotropic_z)) {
    if ((bool)m_config.getItem(SECTION_model, KEY_scale_isotropic_z)) {
      process_model = true;
      scale = SCALE_ISOTROPIC_Z;
    }
  }
  if (m_config.findItem(SECTION_model, KEY_scale_anisotropic)) {
    if ((bool)m_config.getItem(SECTION_model, KEY_scale_anisotropic)) {
      process_model = true;
      scale = SCALE_ANISOTROPIC;
    }
  }
  
  if (process_model == true) {
    scale_object(m_static_objects, scale, align, ignore_minus_z);
  }


  contextCreated();

  m_initialised = true;
  return 0;
}
Ejemplo n.º 16
0
/**
 * Process entry
 *
 * @param argc - number of arguments
 * @param argv - pointer to pointers of arguments
 */
int main( int argc, char **argv )
{
	try
	{
		PieDock::Settings settings;
		char *menuName = 0;

		// parse arguments
		{
			char *binary = basename( *argv );

			while( --argc )
				if( **++argv == '-' )
					switch( *((*argv)+1) )
					{
						default:
							std::cerr << "Skipping unknown flag '" <<
								*((*argv)+1) << "'" << std::endl;
							break;
						case '?':
						case 'h':
							std::cout <<
								binary << " [hvrm]" << std::endl <<
								"\t-h         this help" << std::endl <<
								"\t-v         show version" << std::endl <<
								"\t-r FILE    path and name of alternative " <<
									"configuration file" << std::endl <<
								"\t-m [MENU]  show already running " <<
									"instance" << std::endl;
							return 0;
						case 'v':
							std::cout <<
								binary << " 1.6.7" <<
								std::endl <<
								"Copyright (c) 2007-2014" <<
								std::endl <<
								"Markus Fisch <*****@*****.**>" <<
								std::endl <<
								std::endl <<
								"Tatiana Azundris <*****@*****.**>" <<
								std::endl <<
								"* Modifier masks for key control" <<
								std::endl <<
								std::endl <<
								"Jonas Gehring <*****@*****.**>" <<
								std::endl <<
								"* Custom button actions for menus and icons" <<
								std::endl <<
								"* Better tokenization of settings statements" <<
								std::endl <<
								std::endl <<
								"Licensed under the MIT license:" <<
								std::endl <<
								"http://www.opensource.org/licenses/mit-license.php" <<
								std::endl;
							return 0;
						case 'r':
							if( !--argc )
								throw std::invalid_argument(
									"missing FILE argument" );
							settings.setConfigurationFile( *++argv );
							break;
						case 'm':
							if( argc > 1 &&
								**(argv+1) != '-' )
							{
								--argc;
								menuName = *++argv;
							}
							break;
					}
				else
					std::cerr << "skipping unknown argument \"" <<
						*argv << "\"" << std::endl;

			if( settings.getConfigurationFile().empty() )
				settings.setConfigurationFileFromBinary( binary );
		}

		switch( fork() )
		{
			default:
				// terminate parent process to detach from shell
				return 0;
			case 0:
				// pursue in child process
				break;
			case -1:
				throw std::runtime_error( "cannot fork" );
		}

#ifdef HAVE_GTK
		gtk_init( &argc, &argv );
#endif

#ifdef HAVE_KDE
		QApplication q( argc, argv );
#endif

		// always open display after fork
		PieDock::Application a( settings );

		// if another instance is already running, wake it
		if( a.remote( menuName ) )
			return 0;

		// obtain new process group
		setsid();

		signal( SIGCHLD, signalHandler );
		signal( SIGHUP, signalHandler );
		signal( SIGINT, signalHandler );
		signal( SIGTERM, signalHandler );

#ifdef HAVE_KDE
		int r = a.run( &stop );
		q.quit();
		return r;
#else
		return a.run( &stop );
#endif
	}
	catch( std::exception &e )
	{
		std::cerr << "error: " << e.what() << std::endl;

		return -1;
	}
}
void AttitudePositionEstimatorEKF::print_status()
{
	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
	math::Matrix<3, 3> R = q.to_dcm();
	math::Vector<3> euler = R.to_euler();

	printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
	       (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));

	// State vector:
	// 0-3: quaternions (q0, q1, q2, q3)
	// 4-6: Velocity - m/sec (North, East, Down)
	// 7-9: Position - m (North, East, Down)
	// 10-12: Delta Angle bias - rad (X,Y,Z)
	// 13:    Delta Velocity Bias - m/s (Z)
	// 14-15: Wind Vector  - m/sec (North,East)
	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)

	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset,
	       (double)_baro_gps_offset);
	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
	       (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
	       (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
	       (double)_ekf->correctedDelAng.z);
	printf("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1],
	       (double)_ekf->states[2], (double)_ekf->states[3]);
	printf("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5],
	       (double)_ekf->states[6]);
	printf("states (pos m)      [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8],
	       (double)_ekf->states[9]);
	printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11],
	       (double)_ekf->states[12]);

	if (EKF_STATE_ESTIMATES == 23) {
		printf("states (accel offs)   [13]: %8.4f\n", (double)_ekf->states[13]);
		printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
		printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17],
		       (double)_ekf->states[18]);
		printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20],
		       (double)_ekf->states[21]);
		printf("states (terrain)      [22]: %8.4f\n", (double)_ekf->states[22]);

	} else {
		printf("states (wind)      [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
		printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16],
		       (double)_ekf->states[17]);
		printf("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19],
		       (double)_ekf->states[20]);
	}

	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
	       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
	       (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
	       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
	       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
	       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
	       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
	       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
	       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
	       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
	       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
}
Ejemplo n.º 18
0
void GISTrainer::train(size_t iter, double tol) {
    if (!m_params || !m_es)
        throw runtime_error("Can not train on an empty model");

    init_trainer();

    // now enter iterations
    const double LOG_ZERO = log(numeric_limits<double>::min());
    double old_loglikelihood = 99999;
    double new_loglikelihood = 0.0;
    double acc;
    size_t correct;
    size_t best_oid;
    vector<double> q(m_n_outcomes); // q(y|x)
    boost::timer t;
    size_t niter = 0;

    display("");
    display("Starting GIS iterations...");
    display("Number of Predicates: %d", m_params->size());
    display("Number of Outcomes:   %d", m_n_outcomes);
    display("Number of Parameters: %d", m_n_theta);
    display("Tolerance:            %E", tol);
    display("Gaussian Penalty:     %s", (m_sigma2?"on":"off"));
#if defined(NDEBUG)
    display("Optimized version");
#endif
    display("iters   loglikelihood    training accuracy   heldout accuracy");
    display("=============================================================");

    for (; niter < iter;) {
        new_loglikelihood = 0.0;
        correct = 0;

        // computer modifiers for all features from training data
        for (vector<Event>::iterator it = m_es->begin();
                it != m_es->end(); ++it) {
            best_oid = eval(it->m_context, it->context_size(), q);
            if (best_oid == it->m_outcome)
                correct += it->m_count;
            // TODO:optimize the code
            // calculate Eq<f_i> = \sum q(y|x) * Count(f_i) * f_i(x, y) 
            // (need not being divided by N)
            for (size_t i = 0; i < it->context_size(); ++i) {
                size_t pid = it->m_context[i].first;
                double fval = it->m_context[i].second;
                vector<pair<size_t, size_t> >& param = (*m_params)[pid];
                for (size_t j = 0; j < param.size(); ++j) {
                    size_t oid = param[j].first;
                    (*m_modifiers)[pid][j] += q[oid] * it->m_count * fval;
                    // binary case: (*m_modifiers)[pid][j] += q[oid] * it->m_count;
                }
            }
            assert(finite(q[it->m_outcome]));
            double t = log(q[it->m_outcome]);
            new_loglikelihood += (finite(t) ? t : LOG_ZERO) * it->m_count;
            assert(finite(new_loglikelihood));
        }
        acc = correct/double(m_N);

        // compute the new parameter values
        if (m_sigma2) { // applying Gaussian penality
            for (size_t pid = 0; pid < m_params->size(); ++pid) {
                vector<pair<size_t, size_t> >& param = (*m_params)[pid];
                for (size_t i = 0; i < param.size(); ++i) {
                    size_t fid = param[i].second;
                    m_theta[fid] += newton((*m_modifiers)[pid][i],
                            (*m_observed_expects)[pid][i], fid);
                    (*m_modifiers)[pid][i] = 0.0; // clear modifiers for next iteration
                }
            }
        } else {
            for (size_t pid = 0; pid < m_params->size(); ++pid) {
                vector<pair<size_t, size_t> >& param = (*m_params)[pid];
                for (size_t i = 0; i < param.size(); ++i) {
                    if ((*m_modifiers)[pid][i] != 0.0) { 
                        m_theta[param[i].second] += 
                            ((*m_observed_expects)[pid][i] -
                             log((*m_modifiers)[pid][i])) / m_correct_constant;
                        (*m_modifiers)[pid][i] = 0.0; // clear modifiers for next iteration
                    } else {
                        // E_q == 0 means feature value is 0, which means
                        // update for this parameter will always be zero,
                        // hence can be ignored.
                    }
                }
            }
        }

        ++niter;
        display("%3d\t%E\t  %.3f%%\t     %s",
                niter , (new_loglikelihood/m_N) , (acc*100) ,  "N/A");
        if (fabs((old_loglikelihood - new_loglikelihood)/old_loglikelihood) < tol) {
            display("Training terminats succesfully in %.2f seconds", t.elapsed());
            break;
        }
        old_loglikelihood = new_loglikelihood;
    }
    if (niter >= iter)
        display("Maximum numbers of %d iterations reached in %.2f seconds", iter , t.elapsed());

    // kill a bunch of these big objects now that we don't need them
    m_modifiers.reset();
    m_observed_expects.reset();
}
Ejemplo n.º 19
0
Quaternion Quaternion::getConjugated() const
{
    Quaternion q(*this);
    q.conjugate();
    return q;
}
Ejemplo n.º 20
0
void
MulticopterAttitudeControl::task_main()
{

	/*
	 * do subscriptions
	 */
	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
	_battery_status_sub = orb_subscribe(ORB_ID(battery_status));

	_gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);

	for (unsigned s = 0; s < _gyro_count; s++) {
		_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
	}

	_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));

	/* initialize parameters cache */
	parameters_update();

	/* wakeup source: gyro data from sensor selected by the sensor app */
	px4_pollfd_struct_t poll_fds = {};
	poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
	poll_fds.events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = px4_poll(&poll_fds, 1, 100);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("mc att ctrl: poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		perf_begin(_loop_perf);

		/* run controller on gyro changes */
		if (poll_fds.revents & POLLIN) {
			static uint64_t last_run = 0;
			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
			if (dt < 0.002f) {
				dt = 0.002f;

			} else if (dt > 0.02f) {
				dt = 0.02f;
			}

			/* copy gyro data */
			orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);

			/* check for updates in other topics */
			parameter_update_poll();
			vehicle_control_mode_poll();
			arming_status_poll();
			vehicle_manual_poll();
			vehicle_status_poll();
			vehicle_motor_limits_poll();
			battery_status_poll();
			control_state_poll();
			sensor_correction_poll();

			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't
			 * even bother running the attitude controllers */
			if (_v_control_mode.flag_control_rattitude_enabled) {
				if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
				    fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
					_v_control_mode.flag_control_attitude_enabled = false;
				}
			}

			if (_v_control_mode.flag_control_attitude_enabled) {

				if (_ts_opt_recovery == nullptr) {
					// the  tailsitter recovery instance has not been created, thus, the vehicle
					// is not a tailsitter, do normal attitude control
					control_attitude(dt);

				} else {
					vehicle_attitude_setpoint_poll();
					_thrust_sp = _v_att_sp.thrust;
					math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
					math::Quaternion q_sp(&_v_att_sp.q_d[0]);
					_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
					_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);

					/* limit rates */
					for (int i = 0; i < 3; i++) {
						_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
					}
				}

				/* publish attitude rates setpoint */
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();

				if (_v_rates_sp_pub != nullptr) {
					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

				} else if (_rates_sp_id) {
					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
				}

				//}

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_control_mode.flag_control_manual_enabled) {
					/* manual rates control - ACRO mode */
					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
								    _manual_control_sp.r).emult(_params.acro_rate_max);
					_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);

					/* publish attitude rates setpoint */
					_v_rates_sp.roll = _rates_sp(0);
					_v_rates_sp.pitch = _rates_sp(1);
					_v_rates_sp.yaw = _rates_sp(2);
					_v_rates_sp.thrust = _thrust_sp;
					_v_rates_sp.timestamp = hrt_absolute_time();

					if (_v_rates_sp_pub != nullptr) {
						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

					} else if (_rates_sp_id) {
						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
					}

				} else {
					/* attitude controller disabled, poll rates setpoint topic */
					vehicle_rates_setpoint_poll();
					_rates_sp(0) = _v_rates_sp.roll;
					_rates_sp(1) = _v_rates_sp.pitch;
					_rates_sp(2) = _v_rates_sp.yaw;
					_thrust_sp = _v_rates_sp.thrust;
				}
			}

			if (_v_control_mode.flag_control_rates_enabled) {
				control_attitude_rates(dt);

				/* publish actuator controls */
				_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
				_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
				_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
				_actuators.control[7] = _v_att_sp.landing_gear;
				_actuators.timestamp = hrt_absolute_time();
				_actuators.timestamp_sample = _ctrl_state.timestamp;

				/* scale effort by battery status */
				if (_params.bat_scale_en && _battery_status.scale > 0.0f) {
					for (int i = 0; i < 4; i++) {
						_actuators.control[i] *= _battery_status.scale;
					}
				}

				_controller_status.roll_rate_integ = _rates_int(0);
				_controller_status.pitch_rate_integ = _rates_int(1);
				_controller_status.yaw_rate_integ = _rates_int(2);
				_controller_status.timestamp = hrt_absolute_time();

				if (!_actuators_0_circuit_breaker_enabled) {
					if (_actuators_0_pub != nullptr) {

						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
						perf_end(_controller_latency_perf);

					} else if (_actuators_id) {
						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
					}

				}

				/* publish controller status */
				if (_controller_status_pub != nullptr) {
					orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);

				} else {
					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
				}
			}

			if (_v_control_mode.flag_control_termination_enabled) {
				if (!_vehicle_status.is_vtol) {

					_rates_sp.zero();
					_rates_int.zero();
					_thrust_sp = 0.0f;
					_att_control.zero();


					/* publish actuator controls */
					_actuators.control[0] = 0.0f;
					_actuators.control[1] = 0.0f;
					_actuators.control[2] = 0.0f;
					_actuators.control[3] = 0.0f;
					_actuators.timestamp = hrt_absolute_time();
					_actuators.timestamp_sample = _ctrl_state.timestamp;

					if (!_actuators_0_circuit_breaker_enabled) {
						if (_actuators_0_pub != nullptr) {

							orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
							perf_end(_controller_latency_perf);

						} else if (_actuators_id) {
							_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
						}
					}

					_controller_status.roll_rate_integ = _rates_int(0);
					_controller_status.pitch_rate_integ = _rates_int(1);
					_controller_status.yaw_rate_integ = _rates_int(2);
					_controller_status.timestamp = hrt_absolute_time();

					/* publish controller status */
					if (_controller_status_pub != nullptr) {
						orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);

					} else {
						_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
					}

					/* publish attitude rates setpoint */
					_v_rates_sp.roll = _rates_sp(0);
					_v_rates_sp.pitch = _rates_sp(1);
					_v_rates_sp.yaw = _rates_sp(2);
					_v_rates_sp.thrust = _thrust_sp;
					_v_rates_sp.timestamp = hrt_absolute_time();

					if (_v_rates_sp_pub != nullptr) {
						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

					} else if (_rates_sp_id) {
						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
					}
				}
			}
		}

		perf_end(_loop_perf);
	}

	_control_task = -1;
	return;
}
Ejemplo n.º 21
0
Quaternion Quaternion::getNormalized() const
{
    Quaternion q(*this);
    q.normalize();
    return q;
}
Ejemplo n.º 22
0
void ConvertRotationToBull(const QAngle& angles, btMatrix3x3& bull) {
	RadianEuler radian(angles);
	Quaternion q(radian);
	btQuaternion quat(q.x, q.z, -q.y, q.w);
	bull.setRotation(quat);
}
void ADFun<Base>::SparseJacobianCase(
	const std::set<size_t>&  set_type        ,
	const VectorBase&        x               , 
	const VectorSet&         p               ,
	VectorBase&              jac             )
{
	typedef CppAD::vector<size_t> SizeVector;
	typedef CppAD::vectorBool     VectorBool;
	size_t i, j, k;

	size_t m = Range();
	size_t n = Domain();

	// some values
	const Base zero(0);
	const Base one(1);

	// check VectorSet is Simple Vector class with sets for elements
	CheckSimpleVector<std::set<size_t>, VectorSet>(
		one_element_std_set<size_t>(), two_element_std_set<size_t>()
	);

	// check VectorBase is Simple Vector class with Base type elements
	CheckSimpleVector<Base, VectorBase>();

	CPPAD_ASSERT_KNOWN(
		x.size() == n,
		"SparseJacobian: size of x not equal domain dimension for f"
	); 
	CPPAD_ASSERT_KNOWN(
		p.size() == m,
		"SparseJacobian: using sets and size of p "
		"not equal range dimension for f"
	); 
	CPPAD_ASSERT_UNKNOWN(jac.size() == m * n); 

	// point at which we are evaluating the Jacobian
	Forward(0, x);

	// initialize the return value
	for(i = 0; i < m; i++)
		for(j = 0; j < n; j++)
			jac[i * n + j] = zero;

	// create a copy of the transpose sparsity pattern
	VectorSet q(n);
	std::set<size_t>::const_iterator itr_i, itr_j;
	for(i = 0; i < m; i++)
	{	itr_j = p[i].begin();
		while( itr_j != p[i].end() )
		{	j = *itr_j++;
			q[j].insert(i);
		}
	}	

	if( n <= m )
	{	// use forward mode ----------------------------------------
	
		// initial coloring
		SizeVector color(n);
		for(j = 0; j < n; j++)
			color[j] = j;

		// See GreedyPartialD2Coloring Algorithm Section 3.6.2 of
		// Graph Coloring in Optimization Revisited by
		// Assefaw Gebremedhin, Fredrik Maane, Alex Pothen
		VectorBool forbidden(n);
		for(j = 0; j < n; j++)
		{	// initial all colors as ok for this column
			for(k = 0; k < n; k++)
				forbidden[k] = false;

			// for each row connected to column j
			itr_i = q[j].begin();
			while( itr_i != q[j].end() )
			{	i = *itr_i++;
				// for each column connected to row i
				itr_j = p[i].begin();
				while( itr_j != p[i].end() )
				{	// if this is not j, forbid it
					k = *itr_j++;
					forbidden[ color[k] ] = (k != j);
				}
			}
			k = 0;
			while( forbidden[k] && k < n )
			{	k++;
				CPPAD_ASSERT_UNKNOWN( k < n );
			}
			color[j] = k;
		}
		size_t n_color = 1;
		for(k = 0; k < n; k++) 
			n_color = std::max(n_color, color[k] + 1);

		// direction vector for calls to forward
		VectorBase dx(n);

		// location for return values from Reverse
		VectorBase dy(m);

		// loop over colors
		size_t c;
		for(c = 0; c < n_color; c++)
		{	// determine all the colums with this color
			for(j = 0; j < n; j++)
			{	if( color[j] == c )
					dx[j] = one;
				else	dx[j] = zero;
			}
			// call forward mode for all these columns at once
			dy = Forward(1, dx);

			// set the corresponding components of the result
			for(j = 0; j < n; j++) if( color[j] == c )
			{	itr_i = q[j].begin();
				while( itr_i != q[j].end() )
				{	i = *itr_i++;
					jac[i * n + j] = dy[i];
				}
			}
		}
	}
	else
	{	// use reverse mode ----------------------------------------
	
		// initial coloring
		SizeVector color(m);
		for(i = 0; i < m; i++)
			color[i] = i;

		// See GreedyPartialD2Coloring Algorithm Section 3.6.2 of
		// Graph Coloring in Optimization Revisited by
		// Assefaw Gebremedhin, Fredrik Maane, Alex Pothen
		VectorBool forbidden(m);
		for(i = 0; i < m; i++)
		{	// initial all colors as ok for this row
			for(k = 0; k < m; k++)
				forbidden[k] = false;

			// for each column connected to row i
			itr_j = p[i].begin();
			while( itr_j != p[i].end() )
			{	j = *itr_j++;	
				// for each row connected to column j
				itr_i = q[j].begin();
				while( itr_i != q[j].end() )
				{	// if this is not i, forbid it
					k = *itr_i++;
					forbidden[ color[k] ] = (k != i);
				}
			}
			k = 0;
			while( forbidden[k] && k < m )
			{	k++;
				CPPAD_ASSERT_UNKNOWN( k < n );
			}
			color[i] = k;
		}
		size_t n_color = 1;
		for(k = 0; k < m; k++) 
			n_color = std::max(n_color, color[k] + 1);

		// weight vector for calls to reverse
		VectorBase w(m);

		// location for return values from Reverse
		VectorBase dw(n);

		// loop over colors
		size_t c;
		for(c = 0; c < n_color; c++)
		{	// determine all the rows with this color
			for(i = 0; i < m; i++)
			{	if( color[i] == c )
					w[i] = one;
				else	w[i] = zero;
			}
			// call reverse mode for all these rows at once
			dw = Reverse(1, w);

			// set the corresponding components of the result
			for(i = 0; i < m; i++) if( color[i] == c )
			{	itr_j = p[i].begin();
				while( itr_j != p[i].end() )
				{	j = *itr_j++;
					jac[i * n + j] = dw[j];
				}
			}
		}
	}
}
Ejemplo n.º 24
0
void ConvertRotationToBull(const QAngle& angles, btQuaternion& bull) {
	RadianEuler radian(angles);
	Quaternion q(radian);
	bull.setValue(q.x, q.z, -q.y, q.w);
}
Ejemplo n.º 25
0
btScalar btKart::rayCast(unsigned int index)
{
    btWheelInfo &wheel = m_wheelInfo[index];

    // Work around a bullet problem: when using a convex hull the raycast
    // would sometimes hit the chassis (which does not happen when using a
    // box shape). Therefore set the collision mask in the chassis body so
    // that it is not hit anymore.
    short int old_group=0;
    if(m_chassisBody->getBroadphaseHandle())
    {
        old_group = m_chassisBody->getBroadphaseHandle()
                                 ->m_collisionFilterGroup;
        m_chassisBody->getBroadphaseHandle()->m_collisionFilterGroup = 0;
    }

    updateWheelTransformsWS( wheel,false);

    btScalar depth = -1;

    btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius
                    + wheel.m_maxSuspensionTravelCm*0.01f;

    btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
    const btVector3& source = wheel.m_raycastInfo.m_hardPointWS;
    wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
    const btVector3& target = wheel.m_raycastInfo.m_contactPointWS;

    btScalar param = btScalar(0.);

    btVehicleRaycaster::btVehicleRaycasterResult rayResults;

    btAssert(m_vehicleRaycaster);

    void* object = m_vehicleRaycaster->castRay(source,target,rayResults);

    wheel.m_raycastInfo.m_groundObject = 0;

    if (object)
    {
        param = rayResults.m_distFraction;
        depth = raylen * rayResults.m_distFraction;
        wheel.m_raycastInfo.m_contactNormalWS  = rayResults.m_hitNormalInWorld;
        wheel.m_raycastInfo.m_contactNormalWS.normalize();
        wheel.m_raycastInfo.m_isInContact = true;
        ///@todo for driving on dynamic/movable objects!;
        wheel.m_raycastInfo.m_groundObject = &getFixedBody();

        btScalar hitDistance = param*raylen;
        wheel.m_raycastInfo.m_suspensionLength =
            hitDistance - wheel.m_wheelsRadius;
        //clamp on max suspension travel

        btScalar minSuspensionLength = wheel.getSuspensionRestLength()
                                - wheel.m_maxSuspensionTravelCm*btScalar(0.01);
        btScalar maxSuspensionLength = wheel.getSuspensionRestLength()
                                + wheel.m_maxSuspensionTravelCm*btScalar(0.01);
        if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength)
        {
            wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
        }
        if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength)
        {
            wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
        }

        wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld;

        btScalar denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(
                                      wheel.m_raycastInfo.m_wheelDirectionWS );

        btVector3 chassis_velocity_at_contactPoint;
        btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS
                         - getRigidBody()->getCenterOfMassPosition();

        chassis_velocity_at_contactPoint =
            getRigidBody()->getVelocityInLocalPoint(relpos);

        btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(
                                            chassis_velocity_at_contactPoint );

        if ( denominator >= btScalar(-0.1))
        {
            wheel.m_suspensionRelativeVelocity = btScalar(0.0);
            wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
        }
        else
        {
            btScalar inv = btScalar(-1.) / denominator;
            wheel.m_suspensionRelativeVelocity = projVel * inv;
            wheel.m_clippedInvContactDotSuspension = inv;
        }

    } else
    {
        //put wheel info as in rest position
        wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
        wheel.m_suspensionRelativeVelocity = btScalar(0.0);
        wheel.m_raycastInfo.m_contactNormalWS =
            - wheel.m_raycastInfo.m_wheelDirectionWS;
        wheel.m_clippedInvContactDotSuspension = btScalar(1.0);
    }

#define USE_VISUAL
#ifndef USE_VISUAL
    m_visual_contact_point[index] = rayResults.m_hitPointInWorld;
#else
    if(index==2 || index==3)
    {
        btTransform chassisTrans = getChassisWorldTransform();
        if (getRigidBody()->getMotionState())
        {
            getRigidBody()->getMotionState()->getWorldTransform(chassisTrans);
        }
        btQuaternion q(m_visual_rotation, 0, 0);
        btQuaternion rot_new = chassisTrans.getRotation() * q;
        chassisTrans.setRotation(rot_new);
        btVector3 pos = m_kart->getKartModel()->getWheelGraphicsPosition(index);
        pos.setZ(pos.getZ()*0.9f);
        btVector3 source = chassisTrans( pos );
        btVector3 target = source + rayvector;
        btVehicleRaycaster::btVehicleRaycasterResult rayResults;

        void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
        m_visual_contact_point[index] = rayResults.m_hitPointInWorld;
        m_visual_contact_point[index-2] = source;
        m_visual_wheels_touch_ground &= (object!=NULL);
    }
#endif

    if(m_chassisBody->getBroadphaseHandle())
    {
        m_chassisBody->getBroadphaseHandle()->m_collisionFilterGroup
            = old_group;
    }


    return depth;

}   // rayCast
Ejemplo n.º 26
0
void ConvertRotationToHL(const btQuaternion& quat, QAngle& hl)
{
	Quaternion q(quat.getX(), -quat.getZ(), quat.getY(), quat.getW());
	RadianEuler radian(q);
	hl = radian.ToQAngle();
}
Ejemplo n.º 27
0
TEST_F(NasaPoly1Test, Copy)
{
    NasaPoly1 q(poly);
    testEquivalent(poly, q);
}
Ejemplo n.º 28
0
/*************************************************************************
Problem testing
*************************************************************************/
static void testproblem(const ap::real_2d_array& a, int m, int n)
{
    int i;
    int j;
    int k;
    double mx;
    ap::real_2d_array b;
    ap::real_1d_array taub;
    ap::real_2d_array q;
    ap::real_2d_array r;
    ap::real_2d_array q2;
    double v;

    
    //
    // MX - estimate of the matrix norm
    //
    mx = 0;
    for(i = 0; i <= m-1; i++)
    {
        for(j = 0; j <= n-1; j++)
        {
            if( ap::fp_greater(fabs(a(i,j)),mx) )
            {
                mx = fabs(a(i,j));
            }
        }
    }
    if( ap::fp_eq(mx,0) )
    {
        mx = 1;
    }
    
    //
    // Test decompose-and-unpack error
    //
    makeacopy(a, m, n, b);
    rmatrixqr(b, m, n, taub);
    rmatrixqrunpackq(b, m, n, taub, m, q);
    rmatrixqrunpackr(b, m, n, r);
    for(i = 0; i <= m-1; i++)
    {
        for(j = 0; j <= n-1; j++)
        {
            v = ap::vdotproduct(q.getrow(i, 0, m-1), r.getcolumn(j, 0, m-1));
            decomperrors = decomperrors||ap::fp_greater_eq(fabs(v-a(i,j)),threshold);
        }
    }
    for(i = 0; i <= m-1; i++)
    {
        for(j = 0; j <= ap::minint(i, n-1)-1; j++)
        {
            structerrors = structerrors||ap::fp_neq(r(i,j),0);
        }
    }
    for(i = 0; i <= m-1; i++)
    {
        for(j = 0; j <= m-1; j++)
        {
            v = ap::vdotproduct(&q(i, 0), &q(j, 0), ap::vlen(0,m-1));
            if( i==j )
            {
                structerrors = structerrors||ap::fp_greater_eq(fabs(v-1),threshold);
            }
            else
            {
                structerrors = structerrors||ap::fp_greater_eq(fabs(v),threshold);
            }
        }
    }
    
    //
    // Test for other errors
    //
    for(k = 1; k <= m-1; k++)
    {
        rmatrixqrunpackq(b, m, n, taub, k, q2);
        for(i = 0; i <= m-1; i++)
        {
            for(j = 0; j <= k-1; j++)
            {
                othererrors = othererrors||ap::fp_greater(fabs(q2(i,j)-q(i,j)),10*ap::machineepsilon);
            }
        }
    }
}
Ejemplo n.º 29
0
        virtual void queryOp( Request& r ){
            QueryMessage q( r.d() );

            log(3) << "shard query: " << q.ns << "  " << q.query << endl;
            
            if ( q.ntoreturn == 1 && strstr(q.ns, ".$cmd") )
                throw UserException( 8010 , "something is wrong, shouldn't see a command here" );

            ChunkManagerPtr info = r.getChunkManager();
            assert( info );
            
            Query query( q.query );

            vector<shared_ptr<ChunkRange> > shards;
            info->getChunksForQuery( shards , query.getFilter()  );
            
            set<ServerAndQuery> servers;
            for ( vector<shared_ptr<ChunkRange> >::iterator i = shards.begin(); i != shards.end(); i++ ){
                shared_ptr<ChunkRange> c = *i;
                //servers.insert( ServerAndQuery( c->getShard().getConnString() , BSONObj() ) ); // ERH ERH ERH 
                servers.insert( ServerAndQuery( c->getShard().getConnString() , c->getFilter() ) );
            }
            
            if ( logLevel > 4 ){
                StringBuilder ss;
                ss << " shard query servers: " << servers.size() << '\n';
                for ( set<ServerAndQuery>::iterator i = servers.begin(); i!=servers.end(); i++ ){
                    const ServerAndQuery& s = *i;
                    ss << "       " << s.toString() << '\n';
                }
                log() << ss.str();
            }

            ClusteredCursor * cursor = 0;
            
            BSONObj sort = query.getSort();
            
            if ( sort.isEmpty() ){
                // 1. no sort, can just hit them in serial
                cursor = new SerialServerClusteredCursor( servers , q );
            }
            else {
                int shardKeyOrder = info->getShardKey().canOrder( sort );
                if ( shardKeyOrder ){
                    // 2. sort on shard key, can do in serial intelligently
                    set<ServerAndQuery> buckets;
                    for ( vector<shared_ptr<ChunkRange> >::iterator i = shards.begin(); i != shards.end(); i++ ){
                        shared_ptr<ChunkRange> s = *i;
                        buckets.insert( ServerAndQuery( s->getShard().getConnString() , s->getFilter() , s->getMin() ) );
                    }
                    cursor = new SerialServerClusteredCursor( buckets , q , shardKeyOrder );
                }
                else {
                    // 3. sort on non-sharded key, pull back a portion from each server and iterate slowly
                    cursor = new ParallelSortClusteredCursor( servers , q , sort );
                }
            }

            assert( cursor );
            
            log(5) << "   cursor type: " << cursor->type() << endl;
            shardedCursorTypes.hit( cursor->type() );
            
            if ( query.isExplain() ){
                BSONObj explain = cursor->explain();
                replyToQuery( 0 , r.p() , r.m() , explain );
                delete( cursor );
                return;
            }

            ShardedClientCursorPtr cc (new ShardedClientCursor( q , cursor ));
            if ( ! cc->sendNextBatch( r ) ){
                return;
            }
            log(6) << "storing cursor : " << cc->getId() << endl;
            cursorCache.store( cc );
        }
Ejemplo n.º 30
0
    static bool receivedQuery(Client& c, DbResponse& dbresponse, Message& m ) {
        bool ok = true;
        MSGID responseTo = m.header()->id;

        DbMessage d(m);
        QueryMessage q(d);
        auto_ptr< Message > resp( new Message() );

        CurOp& op = *(c.curop());

        shared_ptr<AssertionException> ex;

        try {
            if (!NamespaceString::isCommand(d.getns())) {
                // Auth checking for Commands happens later.
                Status status = cc().getAuthorizationManager()->checkAuthForQuery(d.getns());
                uassert(16550, status.reason(), status.isOK());
            }
            dbresponse.exhaustNS = runQuery(m, q, op, *resp);
            verify( !resp->empty() );
        }
        catch ( SendStaleConfigException& e ){
            ex.reset( new SendStaleConfigException( e.getns(), e.getInfo().msg, e.getVersionReceived(), e.getVersionWanted() ) );
            ok = false;
        }
        catch ( AssertionException& e ) {
            ex.reset( new AssertionException( e.getInfo().msg, e.getCode() ) );
            ok = false;
        }

        if( ex ){

            op.debug().exceptionInfo = ex->getInfo();
            LOGWITHRATELIMIT {
                log() << "assertion " << ex->toString() << " ns:" << q.ns << " query:" <<
                (q.query.valid() ? q.query.toString() : "query object is corrupt") << endl;
                if( q.ntoskip || q.ntoreturn )
                    log() << " ntoskip:" << q.ntoskip << " ntoreturn:" << q.ntoreturn << endl;
            }

            SendStaleConfigException* scex = NULL;
            if ( ex->getCode() == SendStaleConfigCode ) scex = static_cast<SendStaleConfigException*>( ex.get() );

            BSONObjBuilder err;
            ex->getInfo().append( err );
            if( scex ){
                err.append( "ns", scex->getns() );
                scex->getVersionReceived().addToBSON( err, "vReceived" );
                scex->getVersionWanted().addToBSON( err, "vWanted" );
            }
            BSONObj errObj = err.done();

            if( scex ){
                log() << "stale version detected during query over "
                      << q.ns << " : " << errObj << endl;
            }
            else{
                log() << "problem detected during query over "
                      << q.ns << " : " << errObj << endl;
            }

            BufBuilder b;
            b.skip(sizeof(QueryResult));
            b.appendBuf((void*) errObj.objdata(), errObj.objsize());

            // todo: call replyToQuery() from here instead of this!!! see dbmessage.h
            QueryResult * msgdata = (QueryResult *) b.buf();
            b.decouple();
            QueryResult *qr = msgdata;
            qr->_resultFlags() = ResultFlag_ErrSet;
            if( scex ) qr->_resultFlags() |= ResultFlag_ShardConfigStale;
            qr->len = b.len();
            qr->setOperation(opReply);
            qr->cursorId = 0;
            qr->startingFrom = 0;
            qr->nReturned = 1;
            resp.reset( new Message() );
            resp->setData( msgdata, true );

        }

        op.debug().responseLength = resp->header()->dataLen();

        dbresponse.response = resp.release();
        dbresponse.responseTo = responseTo;

        return ok;
    }