Eigen::MatrixXd Kinematics::transform(double x, double y, double z, double roll, double pitch, double yaw) const{ Eigen::MatrixXd trans(4, 4); trans << 1.0, 0.0, 0.0, x, 0.0, 1.0, 0.0, y, 0.0, 0.0, 1.0, z, 0.0, 0.0, 0.0, 1.0; Eigen::MatrixXd ro(4, 4); ro << 1.0, 0.0, 0.0, 0.0, 0.0, cos(roll), -sin(roll), 0.0, 0.0, sin(roll), cos(roll), 0.0, 0.0, 0.0, 0.0, 1.0; Eigen::MatrixXd pi(4, 4); pi << cos(pitch), 0.0, sin(pitch), 0.0, 0.0, 1.0, 0.0, 0.0, -sin(pitch), 0.0, cos(pitch), 0.0, 0.0, 0.0, 0.0, 1.0; Eigen::MatrixXd ya(4, 4); ya << cos(yaw), -sin(yaw), 0.0, 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; Eigen::MatrixXd res = ro * pi * ya * trans; return res; }
ExecStatus Val<View,Offset,shared>::propagate(Space& home, const ModEventDelta&) { Region r(home); ProcessStack xa(r,n); ProcessStack ya(r,n); ValInfo<View>* x = xy; ValInfo<View>* y = xy+n; // Scan x and y for assigned but not yet propagated views for (int i = n; i--; ) { if (x[i].doval()) xa.push(i); if (y[i].doval()) ya.push(i); } do { // Propagate assigned views for x GECODE_ES_CHECK((prop_val<View,Offset,ValInfo<View> > (home,n,x,ox,y,oy,n_na,xa,ya))); // Propagate assigned views for y GECODE_ES_CHECK((prop_val<View,Offset,ValInfo<View> > (home,n,y,oy,x,ox,n_na,ya,xa))); assert(ya.empty()); } while (!xa.empty()); if (n_na == 0) return home.ES_SUBSUMED(*this); return shared ? ES_NOFIX : ES_FIX; }
// Fit model to this random selection of data points. void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M) { unsigned int i ; unsigned int n = x.getRows()/4 ; std::vector<double> xa(4), xb(4); std::vector<double> ya(4), yb(4); unsigned int n2 = n * 2; unsigned int ind2; for(i=0 ; i < 4 ; i++) { ind2 = 2 * ind[i]; xb[i] = x[ind2] ; yb[i] = x[ind2+1] ; xa[i] = x[n2+ind2] ; ya[i] = x[n2+ind2+1] ; } vpHomography aHb ; try { vpHomography::HLM(xb, yb, xa, ya, true, aHb); } catch(...) { aHb.setIdentity(); } M.resize(9); for (i=0 ; i <9 ; i++) { M[i] = aHb.data[i] ; } aHb /= aHb[2][2] ; }
void channel(Home home, const IntVarArgs& x, const SetVarArgs& y) { if (home.failed()) return; ViewArray<Int::CachedView<Int::IntView> > xa(home,x.size()); for (int i=x.size(); i--;) new (&xa[i]) Int::CachedView<Int::IntView>(x[i]); ViewArray<Set::CachedView<Set::SetView> > ya(home,y.size()); for (int i=y.size(); i--;) new (&ya[i]) Set::CachedView<Set::SetView>(y[i]); GECODE_ES_FAIL((Set::Int::ChannelInt<Set::SetView>::post(home,xa,ya))); }
void apply( stk_classic::mesh::Bucket::iterator bucket_i , stk_classic::mesh::Bucket::iterator bucket_j , reduce_type * inout ) const { stk_classic::mesh::BucketArray<field_type> xa( field_x , bucket_i , bucket_j ); stk_classic::mesh::BucketArray<field_type> ya( field_y , bucket_i , bucket_j ); scalar * xi = xa.contiguous_data(); scalar * yi = ya.contiguous_data(); scalar * ye = yi + ya.size(); scalar tmp = 0 ; while ( yi != ye ) { tmp += *xi++ * *yi++ ; } *inout += tmp ; }
void apply( stk_classic::mesh::Bucket::iterator bucket_i , stk_classic::mesh::Bucket::iterator bucket_j ) const { typedef typename stk_classic::mesh::FieldTraits<field_type>::data_type scalar ; stk_classic::mesh::BucketArray<field_type> xa( x_ , bucket_i , bucket_j ); stk_classic::mesh::BucketArray<field_type> ya( y_ , bucket_i , bucket_j ); scalar * xi = xa.contiguous_data(); scalar * yi = ya.contiguous_data(); scalar * ye = yi + ya.size(); //is it worthwhile to optimize by adding special loops for //cases where alpha or beta equal 1.0 or 0.0, or where x==y? while(yi < ye) { *yi = alpha_* *xi++ + beta_* *yi; ++yi; } }
void createAtomicLib() { typedef CG<double> CGD; typedef AD<CGD> ADCGD; /** * Tape model */ std::vector<ADCGD> xa(na_); for (size_t j = 0; j < na_; j++) xa[j] = xa_[j]; CppAD::Independent(xa); std::vector<ADCGD> ya(ns_); atomicFunction(xa, ya); ADFun<CGD> fun; fun.Dependent(ya); /** * Compile */ std::string lName = getAtomicLibName()+(ignoreParameters_ ? "" : "All"); ModelCSourceGen<double> compHelpL(fun, lName); compHelpL.setCreateForwardZero(true); compHelpL.setCreateForwardOne(true); compHelpL.setCreateReverseOne(true); compHelpL.setCreateReverseTwo(true); compHelpL.setTypicalIndependentValues(xa_); if (ignoreParameters_) { std::vector<std::set<size_t> > jacSparAll = extra::jacobianSparsitySet<std::vector<std::set<size_t> > >(fun); std::vector<std::set<size_t> > jacSpar(jacSparAll.size()); for (size_t i = 0; i < jacSparAll.size(); i++) { // only differential information for states and controls std::set<size_t>::const_iterator itEnd = jacSparAll[i].upper_bound(ns_ + nm_ - 1); if (itEnd != jacSparAll[i].begin()) jacSpar[i].insert(jacSparAll[i].begin(), itEnd); } compHelpL.setCustomSparseJacobianElements(jacSpar); std::vector<std::set<size_t> > hessSparAll = extra::hessianSparsitySet<std::vector<std::set<size_t> > >(fun); std::vector<std::set<size_t> > hessSpar(hessSparAll.size()); for (size_t i = 0; i < ns_ + nm_; i++) { std::set<size_t>::const_iterator it = hessSparAll[i].upper_bound(i); // only the lower left side if (it != hessSparAll[i].begin()) hessSpar[i].insert(hessSparAll[i].begin(), it); } compHelpL.setCustomSparseHessianElements(hessSpar); } ModelLibraryCSourceGen<double> compDynHelpL(compHelpL); compDynHelpL.setVerbose(verbose_); SaveFilesModelLibraryProcessor<double>::saveLibrarySourcesTo(compDynHelpL, "sources_" + lName); DynamicModelLibraryProcessor<double> p(compDynHelpL, lName); GccCompiler<double> compiler; if (!compilerFlags_.empty()) compiler.setCompileFlags(compilerFlags_); atomicDynamicLib_.reset(p.createDynamicLibrary(compiler)); /** * load the model */ atomicModel_.reset(atomicDynamicLib_->model(lName)); }
QQuickStateOperation::ActionList QQuickParentChange::actions() { Q_D(QQuickParentChange); if (!d->target || !d->parent) return ActionList(); ActionList actions; QQuickAction a; a.event = this; actions << a; if (d->xString.isValid()) { bool ok = false; qreal x = d->xString.value.numberLiteral(&ok); if (ok) { QQuickAction xa(d->target, QLatin1String("x"), x); actions << xa; } else { QQmlBinding *newBinding = new QQmlBinding(d->xString.value, d->target, qmlContext(this)); QQmlProperty property(d->target, QLatin1String("x")); newBinding->setTarget(property); QQuickAction xa; xa.property = property; xa.toBinding = QQmlAbstractBinding::getPointer(newBinding); xa.fromValue = xa.property.read(); xa.deletableToBinding = true; actions << xa; } } if (d->yString.isValid()) { bool ok = false; qreal y = d->yString.value.numberLiteral(&ok); if (ok) { QQuickAction ya(d->target, QLatin1String("y"), y); actions << ya; } else { QQmlBinding *newBinding = new QQmlBinding(d->yString.value, d->target, qmlContext(this)); QQmlProperty property(d->target, QLatin1String("y")); newBinding->setTarget(property); QQuickAction ya; ya.property = property; ya.toBinding = QQmlAbstractBinding::getPointer(newBinding); ya.fromValue = ya.property.read(); ya.deletableToBinding = true; actions << ya; } } if (d->scaleString.isValid()) { bool ok = false; qreal scale = d->scaleString.value.numberLiteral(&ok); if (ok) { QQuickAction sa(d->target, QLatin1String("scale"), scale); actions << sa; } else { QQmlBinding *newBinding = new QQmlBinding(d->scaleString.value, d->target, qmlContext(this)); QQmlProperty property(d->target, QLatin1String("scale")); newBinding->setTarget(property); QQuickAction sa; sa.property = property; sa.toBinding = QQmlAbstractBinding::getPointer(newBinding); sa.fromValue = sa.property.read(); sa.deletableToBinding = true; actions << sa; } } if (d->rotationString.isValid()) { bool ok = false; qreal rotation = d->rotationString.value.numberLiteral(&ok); if (ok) { QQuickAction ra(d->target, QLatin1String("rotation"), rotation); actions << ra; } else { QQmlBinding *newBinding = new QQmlBinding(d->rotationString.value, d->target, qmlContext(this)); QQmlProperty property(d->target, QLatin1String("rotation")); newBinding->setTarget(property); QQuickAction ra; ra.property = property; ra.toBinding = QQmlAbstractBinding::getPointer(newBinding); ra.fromValue = ra.property.read(); ra.deletableToBinding = true; actions << ra; } } if (d->widthString.isValid()) { bool ok = false; qreal width = d->widthString.value.numberLiteral(&ok); if (ok) { QQuickAction wa(d->target, QLatin1String("width"), width); actions << wa; } else { QQmlBinding *newBinding = new QQmlBinding(d->widthString.value, d->target, qmlContext(this)); QQmlProperty property(d->target, QLatin1String("width")); newBinding->setTarget(property); QQuickAction wa; wa.property = property; wa.toBinding = QQmlAbstractBinding::getPointer(newBinding); wa.fromValue = wa.property.read(); wa.deletableToBinding = true; actions << wa; } } if (d->heightString.isValid()) { bool ok = false; qreal height = d->heightString.value.numberLiteral(&ok); if (ok) { QQuickAction ha(d->target, QLatin1String("height"), height); actions << ha; } else { QQmlBinding *newBinding = new QQmlBinding(d->heightString.value, d->target, qmlContext(this)); QQmlProperty property(d->target, QLatin1String("height")); newBinding->setTarget(property); QQuickAction ha; ha.property = property; ha.toBinding = QQmlAbstractBinding::getPointer(newBinding); ha.fromValue = ha.property.read(); ha.deletableToBinding = true; actions << ha; } } return actions; }