template <bool align> void BgrToYuv422p(const uint8_t * bgr, size_t width, size_t height, size_t bgrStride, uint8_t * y, size_t yStride, uint8_t * u, size_t uStride, uint8_t * v, size_t vStride) { assert((width % 2 == 0) && (width >= DA)); if (align) { assert(Aligned(y) && Aligned(yStride) && Aligned(u) && Aligned(uStride)); assert(Aligned(v) && Aligned(vStride) && Aligned(bgr) && Aligned(bgrStride)); } size_t alignedWidth = AlignLo(width, DA); const size_t A6 = A * 6; for (size_t row = 0; row < height; ++row) { Storer<align> _y(y), _u(u), _v(v); BgrToYuv422p<align, true>(bgr, _y, _u, _v); for (size_t col = DA, colBgr = A6; col < alignedWidth; col += DA, colBgr += A6) BgrToYuv422p<align, false>(bgr + colBgr, _y, _u, _v); Flush(_y, _u, _v); if (width != alignedWidth) { size_t offset = width - DA; Storer<false> _y(y + offset), _u(u + offset / 2), _v(v + offset / 2); BgrToYuv422p<false, true>(bgr + offset * 3, _y, _u, _v); Flush(_y, _u, _v); } y += yStride; u += uStride; v += vStride; bgr += bgrStride; } }
template <bool align> void BgrToYuv444p(const uint8_t * bgr, size_t width, size_t height, size_t bgrStride, uint8_t * y, size_t yStride, uint8_t * u, size_t uStride, uint8_t * v, size_t vStride) { assert(width >= A); if (align) { assert(Aligned(y) && Aligned(yStride) && Aligned(u) && Aligned(uStride)); assert(Aligned(v) && Aligned(vStride) && Aligned(bgr) && Aligned(bgrStride)); } size_t alignedWidth = AlignLo(width, A); const size_t A3 = A * 3; for (size_t row = 0; row < height; ++row) { Storer<align> _y(y), _u(u), _v(v); BgrToYuv444p<align, true>(bgr, _y, _u, _v); for (size_t col = A, colBgr = A3; col < alignedWidth; col += A, colBgr += A3) BgrToYuv444p<align, false>(bgr + colBgr, _y, _u, _v); Flush(_y, _u, _v); if (width != alignedWidth) { size_t col = width - A; Storer<false> _y(y + col), _u(u + col), _v(v + col); BgrToYuv444p<false, true>(bgr + col * 3, _y, _u, _v); Flush(_y, _u, _v); } y += yStride; u += uStride; v += vStride; bgr += bgrStride; } }
template <bool align> void DeinterleaveUv(const uint8_t * uv, size_t uvStride, size_t width, size_t height, uint8_t * u, size_t uStride, uint8_t * v, size_t vStride) { assert(width >= A); if(align) assert(Aligned(uv) && Aligned(uvStride) && Aligned(u) && Aligned(uStride) && Aligned(v) && Aligned(vStride)); size_t alignedWidth = AlignLo(width, A); for(size_t row = 0; row < height; ++row) { Loader<align> _uv(uv); Storer<align> _u(u), _v(v); DeinterleavedUv<align, true>(_uv, _u, _v); for(size_t col = A; col < alignedWidth; col += A) DeinterleavedUv<align, false>(_uv, _u, _v); Flush(_u, _v); if(width != alignedWidth) { Loader<false> _uv(uv + 2*(width - A)); Storer<false> _u(u + width - A), _v(v + width - A); DeinterleavedUv<false, true>(_uv, _u, _v); Flush(_u, _v); } uv += uvStride; u += uStride; v += vStride; } }
tuple2<double, double> *hls_to_rgb(double h, double l, double s) { double m1, m2; if ((s==0.0)) { return (new tuple2<double, double>(3,l,l,l)); } if ((l<=0.5)) { m2 = (l*(1.0+s)); } else { m2 = ((l+s)-(l*s)); } m1 = ((2.0*l)-m2); return (new tuple2<double, double>(3,_v(m1, m2, (h+ONE_THIRD)),_v(m1, m2, h),_v(m1, m2, (h-ONE_THIRD)))); }
void zpt::HTTPReqT::stringify(string& _out) { _out.insert(_out.length(), zpt::method_names[this->__method]); _out.insert(_out.length(), " "); _out.insert(_out.length(), this->__url); if (this->__params.size() != 0) { _out.insert(_out.length(), "?"); bool _first = true; for (auto i : this->__params) { if (!_first) { _out.insert(_out.length(), "&"); } _first = false; string _n(i.first); zpt::url::encode(_n); string _v(i.second); zpt::url::encode(_v); _out.insert(_out.length(), _n); _out.insert(_out.length(), "="); _out.insert(_out.length(), _v); } } _out.insert(_out.length(), " HTTP/1.1"); _out.insert(_out.length(), CRLF); for (auto h : this->__headers) { _out.insert(_out.length(), h.first); _out.insert(_out.length(), ": "); _out.insert(_out.length(), h.second); _out.insert(_out.length(), CRLF); } _out.insert(_out.length(), CRLF); _out.insert(_out.length(), this->__body); }
void RBCamera::set_position(float x,float y,float z) { RBVector4 _v(x-_position.x,y-_position.y,z-_position.z,1); _position.x = x; _position.y = y; _position.z = z; _forward = _forward - _v; }
void ODE_Link::setAbsVelocity(hrp::Vector3& v, hrp::Vector3& w){ dBodySetAngularVel(bodyId, w[0], w[1], w[2]); hrp::Vector3 p; hrp::Matrix33 R; getTransform(p, R); hrp::Vector3 cpos(R*C); hrp::Vector3 _v(v + w.cross(cpos)); dBodySetLinearVel(bodyId, _v[0], _v[1], _v[2]); }
void urdf_traverser::applyTransform(const EigenTransform& t, urdf::Vector3& v) { Eigen::Vector3d _v(v.x, v.y, v.z); Eigen::Quaterniond rot(t.rotation()); //ROS_INFO_STREAM("Rotation: "<<rot); _v = rot * _v; v.x = _v.x(); v.y = _v.y(); v.z = _v.z(); }
/** * set discharge in all interior grid cells (i.e. except ghost layer) * to values specified by parameter functions * Note: unknowns hu and hv represent momentum, while parameters u and v are velocities! */ void SWE_Block::setDischarge(float (*_u)(float, float), float (*_v)(float, float)) { for(int i=1; i<=nx; i++) for(int j=1; j<=ny; j++) { float x = offsetX + (i-0.5f)*dx; float y = offsetY + (j-0.5f)*dy; hu[i][j] = _u(x,y) * h[i][j]; hv[i][j] = _v(x,y) * h[i][j]; }; synchDischargeAfterWrite(); }
/** * set discharge in all interior grid cells (i.e. except ghost layer) * to values specified by parameter functions * Note: unknowns hu and hv represent momentum, while parameters u and v are velocities! */ void SWE_Block::setDischarge(float (*_u)(float, float), float (*_v)(float, float)) { for(int i=nghosts; i<nx+nghosts; i++) for(int j=nghosts; j<ny+nghosts; j++) { float x = offsetX + (i-nghosts+0.5f)*dx; float y = offsetY + (j-nghosts+0.5f)*dy; hu[i][j] = _u(x,y) * h[i][j]; hv[i][j] = _v(x,y) * h[i][j]; }; synchDischargeAfterWrite(); }
void test1() { long double x = operator"" _v(1.2L); assert(x == 2.2L); std::string s = operator"" _w(u"one", 3); assert(s == "boo"); unsigned u = operator"" _w("Hello, World!"); assert(u == 13U); std::complex<double> i = operator"" _i(2.0); assert(i == std::complex<double>(0.0, 2.0)); }
virtual bool update_particle(particle_id_pair const& pi_pair) { BOOST_ASSERT(removed_particles_.end() == removed_particles_.find(pi_pair.first)); std::pair<typename particle_id_pair_set_type::iterator, bool> r( orig_particles_.insert(particle_id_pair( pi_pair.first, particle_type()))); if (r.second && added_particles_.end() == added_particles_.find(pi_pair.first)) { modified_particles_.push_no_duplicate(pi_pair.first); particle_type _v(pc_.get_particle(pi_pair.first).second); std::swap((*r.first).second, _v); } return pc_.update_particle(pi_pair); }
bool update(value_type const& v) { iterator i(std::upper_bound(cntnr_.begin(), cntnr_.end(), v, static_cast<TweakOrdering_ const&>(ord_))); if (i != cntnr_.begin()) { if (*--i == v) { value_type _v(v); std::swap(*i, _v); return false; } ++i; } cntnr_.insert(i, v); return true; }
static void Ublas2Epetra( element_type const& x, vector_ptrtype& v ) { epetra_vector_type& _v( dynamic_cast<epetra_vector_type&>( *v ) ); Epetra_Map v_map( _v.Map() ); DVLOG(2) << "Local size of ublas vector" << x.localSize() << "\n"; DVLOG(2) << "Local size of epetra vector" << v->localSize() << "\n"; const size_type L = v->localSize(); for ( size_type i=0; i<L; i++ ) { DVLOG(2) << "v[" << v_map.GID( i ) << "] = " << "x(" << x.firstLocalIndex() + i << ")=" << x( x.firstLocalIndex() + i ) << "\n"; v->set( v_map.GID( i ), x( x.firstLocalIndex() + i ) ); } }
void TestGradient::mulVM02() { Variable x; Array<const ExprNode> _row1(x,ExprConstant::new_scalar(1)); Array<const ExprNode> _row2(ExprConstant::new_scalar(0),x); const ExprVector& row1=ExprVector::new_(_row1,true); const ExprVector& row2=ExprVector::new_(_row2,true); Array<const ExprNode> _M(row1,row2); const ExprVector& M=ExprVector::new_(_M,false); Array<const ExprNode> _v(x,-x); const ExprVector& v=ExprVector::new_(_v,true); Function f(x,v*M); IntervalVector box(1,Interval(3.0)); IntervalMatrix J=f.jacobian(box); check(J[0][0],Interval(6)); check(J[1][0],Interval(-5)); CPPUNIT_ASSERT(J[0][0].is_superset(Interval(6))); CPPUNIT_ASSERT(J[1][0].is_superset(Interval(-5))); }
docItem::docItem(QList<int> _list, int _curr, int _doc, QWidget *parent):QDialog(parent){ ui.setupUi(this); list = _list; curr = _curr; doc = _doc; QSqlQuery _v(QString("select docs.vid from docs where docs.id = \'%1\'").arg(doc)); _v.next(); vid = _v.value(0).toInt(); ui.spinBox_id_book->hide(); ui.tableWidget_res->setColumnHidden(0, true); ui.tableWidget_res->setColumnHidden(7, true); ui.tableWidget_res->setColumnHidden(8, true); if (vid == 1){ ui.tableWidget_res->setColumnHidden(6, true); ui.radioButton_identifier->hide(); } else if (vid == 2){ ui.lineEdit_place->hide(); ui.label_3->hide(); ui.lineEdit_identifier->setReadOnly(true); } openItem(); readSetting(); connect(ui.pushButton_close, SIGNAL(clicked()), this, SLOT(close())); connect(ui.lineEdit_search, SIGNAL(textEdited(QString)), this, SLOT(searchBook(QString))); connect(ui.tableWidget_res, SIGNAL(doubleClicked(QModelIndex)), this, SLOT(selectBook())); connect(ui.pushButton_save, SIGNAL(clicked()), this, SLOT(saveItem())); connect(ui.pushButton_del, SIGNAL(clicked()), this, SLOT(deleteItem())); connect(ui.pushButton_toFirst, SIGNAL(clicked()), this, SLOT(toFirst())); connect(ui.pushButton_toLast, SIGNAL(clicked()), this, SLOT(toLast())); connect(ui.pushButton_toNext, SIGNAL(clicked()), this, SLOT(toNext())); connect(ui.pushButton_toPrev, SIGNAL(clicked()), this, SLOT(toPrev())); }
virtual bool remove_particle(particle_id_type const& id) { std::pair<typename particle_id_pair_set_type::iterator, bool> r( orig_particles_.insert(particle_id_pair( id, particle_type()))); if (r.second) { particle_type _v(pc_.get_particle(id).second); std::swap((*r.first).second, _v); } if (added_particles_.erase(id) == 0) { modified_particles_.erase(id); const bool result(removed_particles_.push_no_duplicate(id)); BOOST_ASSERT(result); } else { orig_particles_.erase(id); } return pc_.remove_particle(id); }
RH_C_FUNCTION void ON_Light_SetVector(ON_Light* pLight, ON_3DVECTOR_STRUCT v, int which) { const int idxDirection = 0; //const int idxPerpendicularDirection = 1; // no set version - only "get" const int idxLength = 2; const int idxWidth = 3; if( pLight ) { ON_3dVector _v(v.val[0], v.val[1], v.val[2]); switch(which) { case idxDirection: pLight->SetDirection(_v); break; case idxLength: pLight->SetLength(_v); break; case idxWidth: pLight->SetWidth(_v); break; } } }
// \delta v = (I - h J_v - h^2 J_q)^{-1} (h a + h^2 J_q v) // \delta q = h (\delta v + v) void vpSystem::IntegrateDynamicsBackwardEuler(scalar time_step) { if ( m_pRoot->m_bIsGround ) { int i, j, n = m_sState.size(); if ( !n ) return; ForwardDynamics(); RMatrix _a(n,1), _v(n,1); RMatrix _Jq(n,n), _Jv(n,n); for ( i = 0; i < n; i++ ) { _a[i] = m_sState[i].GetAcceleration(); _v[i] = m_sState[i].GetVelocity(); } for ( i = 0; i < n; i++ ) { m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS); ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS); } for ( i = 0; i < n; i++ ) { m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS); ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS); } _Jq *= (time_step * time_step); _Jv *= -time_step; _Jv -= _Jq; for ( i = 0; i < n; i++ ) _Jv[i*(n+1)] += SCALAR_1; _a *= time_step; _a += _Jq * _v; SolveAxEqualB_(_Jv, _a); for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(_v[i] + _a[i]); _a += _v; _a *= time_step; for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _a[i]); } else { int i, j, n = m_sState.size(), m = n + 6; ForwardDynamics(); RMatrix _a(m,1), _v(m,1), _dx(m,1); RMatrix _Jq(m,m), _Jv(m,m), _M = Eye<scalar>(m,m); SE3 _T; se3 gv(SCALAR_0); for ( i = 0; i < n; i++ ) { _a[i] = m_sState[i].GetAcceleration(); _v[i] = m_sState[i].GetVelocity(); } memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3)); memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3)); for ( i = 0; i < n; i++ ) { m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS); ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; for ( j = 0; j < 6; j++ ) _Jq(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS; m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS); } for ( i = 0; i < 6; i++ ) { gv[i] += LIE_EPS; _T = m_pRoot->m_sFrame; m_pRoot->m_sFrame *= Exp(gv); ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jq(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; for ( j = 0; j < 6; j++ ) _Jq(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS; m_pRoot->m_sFrame = _T; gv[i] = SCALAR_0; } for ( i = 0; i < n; i++ ) { m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS); ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS; m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS); } for ( i = 0; i < 6; i++ ) { m_pRoot->m_sV[i] += LIE_EPS; ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS; m_pRoot->m_sV[i] -= LIE_EPS; } _Jq *= (time_step * time_step); _Jv *= time_step; _M -= _Jq; _M -= _Jv; _a *= time_step; _a += _Jq * _v; SolveAxEqualB(_M, _dx, _a); for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(m_sState[i].GetVelocity() + _dx[i]); for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += _dx[n+i]; _dx += _v; _dx *= time_step; for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _dx[i]); m_pRoot->m_sFrame *= Exp(se3(_dx[n], _dx[n+1], _dx[n+2], _dx[n+3], _dx[n+4], _dx[n+5])); } Reparameterize(); }
SUMOReal MSCFModel_Kerner::followSpeed(const MSVehicle* const veh, SUMOReal speed, SUMOReal gap, SUMOReal predSpeed, SUMOReal /*predMaxDecel*/) const { return MIN2(_v(veh, speed, maxNextSpeed(speed, veh), gap, predSpeed), maxNextSpeed(speed, veh)); }
SUMOReal MSCFModel_Kerner::stopSpeed(const MSVehicle* const veh, const SUMOReal speed, SUMOReal gap) const { return MIN2(_v(veh, speed, maxNextSpeed(speed, veh), gap, 0), maxNextSpeed(speed, veh)); }
double MSCFModel_Kerner::followSpeed(const MSVehicle* const veh, double speed, double gap, double predSpeed, double /*predMaxDecel*/, const MSVehicle* const /*pred*/) const { return MIN2(_v(veh, speed, maxNextSpeed(speed, veh), gap, predSpeed), maxNextSpeed(speed, veh)); }
double MSCFModel_Kerner::stopSpeed(const MSVehicle* const veh, const double speed, double gap) const { return MIN2(_v(veh, speed, maxNextSpeed(speed, veh), gap, 0), maxNextSpeed(speed, veh)); }
// \delta v = (I - h J_v)^{-1} (h a) // \delta q = h (\delta v + v) void vpSystem::IntegrateDynamicsBackwardEulerFast(scalar h) { if ( m_pRoot->m_bIsGround ) { int i, j, n = m_sState.size(); if ( !n ) return; ForwardDynamics(); RMatrix _a(n,1), _v(n,1); RMatrix _Jv(n,n); for ( i = 0; i < n; i++ ) { _a[i] = m_sState[i].GetAcceleration(); _v[i] = m_sState[i].GetVelocity(); } for ( i = 0; i < n; i++ ) { m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS); ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS); } RMatrix delv, A = Eye<scalar>(n) - h * _Jv; SolveAxEqualB(A, delv, h * _a); RMatrix delq = h * (delv + _v); for ( i = 0; i < n; i++ ) { m_sState[i].SetVelocity(m_sState[i].GetVelocity() + delv[i]); m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + delq[i]); } } else { int i, j, n = m_sState.size(), m = n + 6; ForwardDynamics(); RMatrix _a(m,1), _v(m,1); RMatrix _Jv(m,m); for ( i = 0; i < n; i++ ) { _a[i] = m_sState[i].GetAcceleration(); _v[i] = m_sState[i].GetVelocity(); } memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3)); memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3)); for ( i = 0; i < n; i++ ) { m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS); ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS; m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS); } for ( i = 0; i < 6; i++ ) { m_pRoot->m_sV[i] += LIE_EPS; ForwardDynamics(); for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS; for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS; m_pRoot->m_sV[i] -= LIE_EPS; } RMatrix delv, A = Eye<scalar>(m) - h * _Jv; SolveAxEqualB(A, delv, h * _a); RMatrix delq = h * (delv + _v); for ( i = 0; i < n; i++ ) { m_sState[i].SetVelocity(m_sState[i].GetVelocity() + delv[i]); m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + delq[i]); } for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += delv[n+i]; m_pRoot->m_sFrame *= Exp(se3(delq[n], delq[n+1], delq[n+2], delq[n+3], delq[n+4], delq[n+5])); } Reparameterize(); }
double MSCFModel_ACC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double /* predMaxDecel */, const MSVehicle* const /* pred */) const { const double desSpeed = MIN2(veh->getLane()->getSpeedLimit(), veh->getMaxSpeed()); return _v(veh, gap2pred, speed, predSpeed, desSpeed, true); }
STRONG_INLINE T operator()(size_t i, size_t j) const { return _u(i,j) + _v(i,j); }
SUMOReal MSCFModel_Wiedemann::followSpeed(const MSVehicle* const veh, SUMOReal /* speed */, SUMOReal gap2pred, SUMOReal predSpeed, SUMOReal /*predMaxDecel*/) const { return _v(veh, predSpeed, gap2pred); }