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; }
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()); // } }
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(); }
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; }
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; }
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(); } }
Quaternion Quaternion::getInversed() const { Quaternion q(*this); q.inverse(); return q; }
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)));"
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"); }
/** * 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; }
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 */
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; } }
// 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; }
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; }
/** * 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"); }
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(); }
Quaternion Quaternion::getConjugated() const { Quaternion q(*this); q.conjugate(); return q; }
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; }
Quaternion Quaternion::getNormalized() const { Quaternion q(*this); q.normalize(); return q; }
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]; } } } } }
void ConvertRotationToBull(const QAngle& angles, btQuaternion& bull) { RadianEuler radian(angles); Quaternion q(radian); bull.setValue(q.x, q.z, -q.y, q.w); }
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
void ConvertRotationToHL(const btQuaternion& quat, QAngle& hl) { Quaternion q(quat.getX(), -quat.getZ(), quat.getY(), quat.getW()); RadianEuler radian(q); hl = radian.ToQAngle(); }
TEST_F(NasaPoly1Test, Copy) { NasaPoly1 q(poly); testEquivalent(poly, q); }
/************************************************************************* 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); } } } }
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 ); }
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; }