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;
            }
        }
Exemple #4
0
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))));
}
Exemple #5
0
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);
}
Exemple #6
0
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();
}
Exemple #9
0
/**
 * 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();
}
Exemple #10
0
/**
 * 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();
}
Exemple #11
0
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));
}
Exemple #12
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);
 }
Exemple #13
0
 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;
 }
Exemple #14
0
    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 ) );
        }
    }
Exemple #15
0
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)));
}
Exemple #16
0
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()));

}
Exemple #17
0
    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);
    }
Exemple #18
0
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));
}
Exemple #22
0
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));
}
Exemple #23
0
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();
}
Exemple #25
0
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);
}