本文整理汇总了C++中q函数的典型用法代码示例。如果您正苦于以下问题：C++ q函数的具体用法？C++ q怎么用？C++ q使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了q函数的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: mexFunction
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;
}
开发者ID:bradking，项目名称:drake，代码行数:64，代码来源:testMultipleTimeLinearPostureConstraintmex.cpp
示例2: qDebug
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 "
//.........这里部分代码省略.........
开发者ID:arxaoc，项目名称:TryOSM，代码行数:101，代码来源:tosmwidget.cpp
示例3: q
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();
}
开发者ID:AdamosX，项目名称:noser-sandbox，代码行数:6，代码来源:Camera.cpp
示例4: CS_ASSERT
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;
}
开发者ID:garinh，项目名称:cs，代码行数:63，代码来源:waterdemo.cpp
示例5: main
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;
}
开发者ID:VinInn，项目名称:ctest，代码行数:92，代码来源:HistoMapReduce.cpp
示例6: q
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)
//.........这里部分代码省略.........
开发者ID:SummerLv，项目名称:paperChapter4，代码行数:101，代码来源:sqldb.cpp
示例7: q
Quaternion Quaternion::getInversed() const
{
Quaternion q(*this);
q.inverse();
return q;
}
开发者ID:jxt1234，项目名称:Simple3D，代码行数:6，代码来源:Quaternion.cpp
示例8: ComputeCorrelatedError
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);
}
开发者ID:radziej，项目名称:findsusyb3，代码行数:77，代码来源:stat.C
示例9: g
#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)));"
开发者ID:gitpan，项目名称:Rinchi-CPlusPlus-Preprocessor，代码行数:29，代码来源:macro_expansion.cpp
示例10: deg2rad
//.........这里部分代码省略.........
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");
}
开发者ID:Zy75，项目名称:ardrone_swarm，代码行数:101，代码来源:ardrone_pose_estimation.cpp
示例11: q
/**
* 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);
}
}
}
}
//.........这里部分代码省略.........
开发者ID:rosswhitfield，项目名称:mantid，代码行数:101，代码来源:MDNormSCD.cpp
示例12: magma_dbpcg
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];
//.........这里部分代码省略.........
开发者ID:xulunfan，项目名称:magma，代码行数:101，代码来源:dbpcg.cpp
示例13: q
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;
}
}
开发者ID:Pleiades-Spiri，项目名称:hector_localization，代码行数:71，代码来源:magnetic.cpp
示例14: assert
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)) {
//.........这里部分代码省略.........
开发者ID:bsmr-worldforge，项目名称:sear，代码行数:101，代码来源:LibModelFile.cpp
示例15: main
//.........这里部分代码省略.........
return 0;
case 'v':
std::cout <<
binary << " 1.6.7" <<
std::endl <<
"Copyright (c) 2007-2014" <<
std::endl <<
"Markus Fisch <[email protected]>" <<
std::endl <<
std::endl <<
"Tatiana Azundris <[email protected]>" <<
std::endl <<
"* Modifier masks for key control" <<
std::endl <<
std::endl <<
"Jonas Gehring <[email protected]>" <<
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;
}
|
客服电话
APP下载
官方微信
请发表评论