コード例 #1
0
ファイル: MechanicsIO.cpp プロジェクト: bremond/siconos
void contactPointProcess(SiconosVector& answer,
                         const Interaction& inter,
                         const T& rel)
{

  answer.resize(14);
  const SiconosVector& posa = *rel.pc1();
  const SiconosVector& posb = *rel.pc2();
  const SiconosVector& nc = *rel.nc();
  const SimpleMatrix& jachqT = *rel.jachqT();
  double id = inter.number();
  double mu = ask<ForMu>(*inter.nslaw());
  SiconosVector cf(jachqT.size(1));
  prod(*inter.lambda(1), jachqT, cf, true);
  answer.setValue(0, mu);

  DEBUG_PRINTF("posa(0)=%g\n", posa(0));
  DEBUG_PRINTF("posa(1)=%g\n", posa(1));
  DEBUG_PRINTF("posa(2)=%g\n", posa(2));


  answer.setValue(1, posa(0));
  answer.setValue(2, posa(1));
  answer.setValue(3, posa(2));
  answer.setValue(4, posb(0));
  answer.setValue(5, posb(1));
  answer.setValue(6, posb(2));
  answer.setValue(7, nc(0));
  answer.setValue(8, nc(1));
  answer.setValue(9, nc(2));
  answer.setValue(10, cf(0));
  answer.setValue(11, cf(1));
  answer.setValue(12, cf(2));
  answer.setValue(13, id);
};
コード例 #2
0
void FirstOrderType1R::computeh(double time, SiconosVector& x, SiconosVector& z, SiconosVector& y)
{
  assert(_pluginh && "FirstOrderType1R::computeOutput() is not linked to a plugin function");

  ((Type1Ptr)(_pluginh->fPtr))(x.size(), &(x)(0), y.size(), &(y)(0), z.size(), &(z)(0));

}
コード例 #3
0
void FirstOrderType1R::computeg(double time, SiconosVector& lambda, SiconosVector& z, SiconosVector& r)
{
  assert(_pluging && "FirstOrderType1R::computeInput() is not linked to a plugin function");

  ((Type1Ptr)(_pluging->fPtr))(lambda.size(), &(lambda)(0), r.size(), &(r)(0), z.size(), &(z)(0));

}
コード例 #4
0
void SphereNEDSSphereNEDSR::computeh(double time, BlockVector& q0, SiconosVector& y)
{


  double q_0 = q0(0);
  double q_1 = q0(1);
  double q_2 = q0(2);
  double q_7 = q0(7);
  double q_8 = q0(8);
  double q_9 = q0(9);

  y.setValue(0, distance(q_0, q_1, q_2, r1, q_7, q_8, q_9, r2));
  //Approximation _Pc1=_Pc2
  _Pc1->setValue(0, (r1 * q_0 + r2 * q_7) / (r1 + r2));
  _Pc1->setValue(1, (r1 * q_1 + r2 * q_8) / (r1 + r2));
  _Pc1->setValue(2, (r1 * q_2 + r2 * q_9) / (r1 + r2));
  _Pc2->setValue(0, (r1 * q_0 + r2 * q_7) / (r1 + r2));
  _Pc2->setValue(1, (r1 * q_1 + r2 * q_8) / (r1 + r2));
  _Pc2->setValue(2, (r1 * q_2 + r2 * q_9) / (r1 + r2));
  _Nc->setValue(0, (q_0 - q_7) / (y.getValue(0) + r1pr2));
  _Nc->setValue(1, (q_1 - q_8) / (y.getValue(0) + r1pr2));
  _Nc->setValue(2, (q_2 - q_9) / (y.getValue(0) + r1pr2));
  //std::cout<<" SphereNEDSSphereNEDSR::computeh dist:"<<y->getValue(0)<<"\n";
  //std::cout<<"_Pc:\n";
  //_Pc->display();
  //std::cout<<"_Nc:\n";
  //_Nc->display();
}
コード例 #5
0
void SimpleMatrix::SolveByLeastSquares(SiconosVector &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares(SiconosVector &B) failed. Not yet implemented for V being a BlockVector.");

  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info = 0;

#ifdef USE_OPTIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::minimal_workspace());
#endif
  if (info != 0)
  {
    std::cout << "info = " << info << std::endl;
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares failed.");
  }
  else
  {
    noalias(*(B.dense())) = ublas::column(tmpB, 0);
  }

}
コード例 #6
0
ファイル: SphereLDS.cpp プロジェクト: siconos/siconos
static
void normalize(SiconosVector& q, unsigned int i)
{
  q.setValue(i, fmod(q.getValue(i), _2PI));

  assert(fabs(q.getValue(i)) - std::numeric_limits<double>::epsilon() >= 0.);
  assert(fabs(q.getValue(i)) < _2PI);
}
コード例 #7
0
ファイル: FirstOrderLinearR.cpp プロジェクト: siconos/siconos
void FirstOrderLinearR::computee(double time, SiconosVector& z, SiconosVector& e)
{

  if (_plugine->fPtr)
  {
    ((FOVecPtr) _plugine->fPtr)(time, e.size(), &(e)(0), z.size(), &(z)(0));
  }
}
コード例 #8
0
void FirstOrderType1R::computeJachx(double time, SiconosVector& x, SiconosVector& z, SimpleMatrix& C)
{
  //
  assert(_pluginJachx && "FirstOrderType1R::computeJacobianH() failed; not linked to a plug-in function.");

  ((Type1Ptr)(_pluginJachx->fPtr))(x.size(), &(x)(0), C.size(0), C.getArray(), z.size(), &(z)(0));

}
コード例 #9
0
void LagrangianCompliantR::computeJachlambda(double time, SiconosVector& q0, SiconosVector& lambda, SiconosVector& z)
{

  if (_pluginJachlambda->fPtr)
  {
    // get vector lambda of the current interaction
    ((FPtr2)_pluginJachlambda->fPtr)(q0.size(), &(q0)(0), lambda.size(), &(lambda)(0), &(*_jachlambda)(0, 0), z.size(), &(z)(0));
    // Copy data that might have been changed in the plug-in call.
  }
}
コード例 #10
0
void SimpleMatrix::PLUForwardBackwardInPlace(SiconosVector &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix PLUForwardBackwardInPlace(V) failed. Not yet implemented for V being a BlockVector.");


  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info;

  if (_num == 1)
  {
    if (!_isPLUFactorized) // call gesv => LU-factorize+solve
    {
      // solve system:
      if (!_ipiv)
        _ipiv.reset(new VInt(size(0)));
      else
        _ipiv->resize(size(0));

      info = lapack::gesv(*mat.Dense, *_ipiv, tmpB);
      _isPLUFactorized = true;

      /*
        ublas::matrix<double> COPY(*mat.Dense);
        ublas::vector<double> S(std::max(size(0),size(1)));
        ublas::matrix<double, ublas::column_major> U(size(0),size(1));
        ublas::matrix<double, ublas::column_major> VT(size(0),size(1));

        int ierr = lapack::gesdd(COPY, S, U, VT);
        printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
      */
      // B now contains solution:
    }
    else // call getrs: only solve using previous lu-factorization
      info = lapack::getrs(*mat.Dense, *_ipiv, tmpB);
  }
  else
  {
    if (!_isPLUFactorized) // call first PLUFactorizationInPlace
    {
      PLUFactorizationInPlace();
    }
    // and then solve
    inplace_solve(*sparse(), tmpB, ublas::lower_tag());
    inplace_solve(ublas::trans(*sparse()), tmpB, ublas::upper_tag());
    info = 0;
  }
  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::PLUForwardBackwardInPlace failed.");
  else
  {
    noalias(*(B.dense())) = ublas::column(tmpB, 0);
  }
}
コード例 #11
0
void LagrangianCompliantR::computeh(double time, SiconosVector& q0, SiconosVector& lambda, SiconosVector& z, SiconosVector& y)
{
  if (_pluginh->fPtr)
  {
    // get vector y of the current interaction

    // Warning: temporary method to have contiguous values in memory, copy of block to simple.
    ((FPtr2)(_pluginh->fPtr))(q0.size(), &(q0)(0), y.size(), &(lambda)(0), &(y)(0), z.size(), &(z)(0));

  }
}
コード例 #12
0
void LagrangianCompliantR::computeJachq(double time, SiconosVector& q0, SiconosVector& lambda, SiconosVector& z)
{

  if (_pluginJachq->fPtr)
  {
    // Warning: temporary method to have contiguous values in memory, copy of block to simple.
    // get vector lambda of the current interaction
    ((FPtr2)(_pluginJachq->fPtr))(q0.size(), &(q0)(0), lambda.size(), &(lambda)(0), &(*_jachq)(0, 0), z.size(), &(z)(0));
    // Copy data that might have been changed in the plug-in call.
  }
}
コード例 #13
0
void LagrangianRheonomousR::computeh(double time, SiconosVector& q, SiconosVector& z, SiconosVector& y)
{
  DEBUG_PRINT(" LagrangianRheonomousR::computeh(double time,Interaction& inter, SP::BlockVector q, SP::BlockVector z)");
  if (_pluginh)
  {
    // arg= time. Unused in this function but required for interface.
    if (_pluginh->fPtr)
    {
      ((FPtr4)(_pluginh->fPtr))(q.size(), &(q)(0), time, y.size(),  &(y)(0), z.size(), &(z)(0));

    }
  }
}
コード例 #14
0
ファイル: FirstOrderLinearR.cpp プロジェクト: siconos/siconos
void FirstOrderLinearR::computeC(double time, SiconosVector& z, SimpleMatrix& C)
{
  if (_pluginJachx->fPtr)
  {
    ((FOMatPtr1)(_pluginJachx->fPtr))(time, C.size(0), C.size(1), &(C)(0, 0), z.size(), &(z)(0));
  }
}
コード例 #15
0
ファイル: BallNewtonEuler.cpp プロジェクト: radarsat1/siconos
  void computeh(double time, BlockVector& q0, SiconosVector& y)
  {

    std::cout <<"my_NewtonEulerR:: computeh" << std:: endl;
    std::cout <<"q0.size() = " << q0.size() << std:: endl;
    double height = q0.getValue(0) - _sBallRadius - q0.getValue(7);
    // std::cout <<"my_NewtonEulerR:: computeh _jachq" << std:: endl;
    // _jachq->display();

    y.setValue(0, height);
    _Nc->setValue(0, 1);
    _Nc->setValue(1, 0);
    _Nc->setValue(2, 0);
    _Pc1->setValue(0, q0.getValue(0) - _sBallRadius);
    _Pc1->setValue(1, q0.getValue(1));
    _Pc1->setValue(2, q0.getValue(2));

    _Pc2->setValue(0,q0.getValue(7));
    _Pc2->setValue(1,q0.getValue(8));
    _Pc2->setValue(2,q0.getValue(9));
    //printf("my_NewtonEulerR N, Pc\n");
    _Nc->display();
    _Pc1->display();
    _Pc2->display();
    std::cout <<"my_NewtonEulerR:: computeh ends" << std:: endl;
  }
コード例 #16
0
ファイル: SiconosVector.cpp プロジェクト: siconos/siconos
void SiconosVector::setVector(unsigned int , const SiconosVector& newV)
{
  if (newV.size() != size())
    SiconosVectorException::selfThrow("SiconosVector::setVector(num,v), unconsistent sizes.");

  *this = newV ;
}
コード例 #17
0
void ControlLinearAdditionalTermsED::addSmoothTerms(DynamicalSystemsGraph& DSG0, const DynamicalSystemsGraph::VDescriptor& dsgVD, const double t, SiconosVector& xdot)
{
    // check whether we have a system with a control input
    if (DSG0.u.hasKey(dsgVD))
    {
        if (DSG0.B.hasKey(dsgVD))
        {
            prod(DSG0.B.getRef(dsgVD), DSG0.u.getRef(dsgVD), xdot, false); // xdot += B*u
        }
        else if (DSG0.pluginU.hasKey(dsgVD))
        {
            DynamicalSystem& ds = *DSG0.bundle(dsgVD);
            SiconosVector& u = DSG0.u.getRef(dsgVD);
            SiconosVector& tmpXdot = DSG0.tmpXdot.getRef(dsgVD);
            ((AdditionalTermsEDfctU)DSG0.pluginU.getRef(dsgVD).fPtr)(t, xdot.size(), ds.getx().getArray(), u.size(), u.getArray(), tmpXdot.getArray(), ds.getz().size(), ds.getz().getArray());
            xdot += tmpXdot; // xdot += g(x, u)
        }
        else
        {
            RuntimeException::selfThrow("ControlLinearAdditionalTermsED :: input u but no B nor pluginU");
        }
    }
    // check whether the DynamicalSystem is an Observer
    if (DSG0.e.hasKey(dsgVD))
    {
        assert(DSG0.L.hasKey(dsgVD));
        prod(*DSG0.L[dsgVD], *DSG0.e[dsgVD], xdot, false); // xdot += -L*e
    }
}
コード例 #18
0
ファイル: FirstOrderLinearR.cpp プロジェクト: siconos/siconos
void FirstOrderLinearR::computeB(double time, SiconosVector& z, SimpleMatrix& B)
{
  if (_pluginJacglambda->fPtr)
  {
    ((FOMatPtr1) _pluginJacglambda->fPtr)(time, B.size(0), B.size(1), &(B)(0, 0), z.size(), &(z)(0));
  }
}
コード例 #19
0
ファイル: FirstOrderLinearR.cpp プロジェクト: siconos/siconos
void FirstOrderLinearR::computeD(double time, SiconosVector& z, SimpleMatrix& D)
{
  if (_pluginJachlambda->fPtr)
  {
    ((FOMatPtr1)(_pluginJachlambda->fPtr))(time, D.size(0), D.size(1), &(D)(0, 0), z.size(), &(z)(0));
  }
}
コード例 #20
0
void LagrangianRheonomousR::computehDot(double time, SiconosVector& q, SiconosVector& z)
{
  if (_pluginhDot && _pluginhDot->fPtr)
  {
    ((FPtr4)(_pluginhDot->fPtr))(q.size(), &(q)(0), time, _hDot->size(),  &(*_hDot)(0), z.size(), &(z)(0));
  }
}
コード例 #21
0
ファイル: FirstOrderLinearR.cpp プロジェクト: siconos/siconos
void FirstOrderLinearR::computeF(double time, SiconosVector& z, SimpleMatrix& F)
{
  if (_pluginf->fPtr)
  {
    ((FOMatPtr1)(_pluginf->fPtr))(time, F.size(0), F.size(1), &(F)(0, 0), z.size(), &(z)(0));
  }
}
コード例 #22
0
ファイル: SiconosVector.cpp プロジェクト: siconos/siconos
void SiconosVector::toBlock(SiconosVector& vOut, unsigned int sizeB, unsigned int startIn, unsigned int startOut) const
{
  // To copy a subBlock of the vector (from position startIn to startIn+sizeB) into vOut (from pos. startOut to startOut+sizeB).
  // Check dim ...
  assert(startIn < size() && "vector toBlock(v1,v2,...): start position in input vector is out of range.");

  assert(startOut < vOut.size() && "vector toBlock(v1,v2,...): start position in output vector is out of range.");

  assert(startIn + sizeB <= size() && "vector toBlock(v1,v2,...): end position in input vector is out of range.");
  assert(startOut + sizeB <= vOut.size() && "vector toBlock(v1,v2,...): end position in output vector is out of range.");

  unsigned int endOut = startOut + sizeB;
  unsigned int numIn = num();
  unsigned int numOut = vOut.num();

  if (numIn == numOut)
  {
    if (numIn == 1) // vIn / vOut are Dense
      noalias(ublas::subrange(*vOut.dense(), startOut, endOut)) = ublas::subrange(*vect.Dense, startIn, startIn + sizeB);
    else // if(numIn == 4)// vIn / vOut are Sparse
      noalias(ublas::subrange(*vOut.sparse(), startOut, endOut)) = ublas::subrange(*vect.Sparse, startIn, startIn + sizeB);
  }
  else // vIn and vout of different types ...
  {
    if (numIn == 1) // vIn Dense
      noalias(ublas::subrange(*vOut.sparse(), startOut, endOut)) = ublas::subrange(*vect.Dense, startIn, startIn + sizeB);
    else // if(numIn == 4)// vIn Sparse
      noalias(ublas::subrange(*vOut.dense(), startOut, endOut)) = ublas::subrange(*vect.Sparse, startIn, startIn + sizeB);
  }
}
コード例 #23
0
/*y = h(X)*/
void NonlinearRelationWithSignInversed::computeh(double t, SiconosVector& x, SiconosVector& lambda, SiconosVector& y)
{


#ifdef SICONOS_DEBUG
  std::cout << "******** NonlinearRelationWithSignInversed::computeh computeh at " << t << std::endl;
#endif


  y.setValue(0, x(0) - 4);
  y.setValue(1, x(1) - 4);
  y.setValue(2, x(0) - 8);
  y.setValue(3, x(1) - 8);
#ifdef SICONOS_DEBUG
  std::cout << "modif heval : \n";
  y.display();
#endif
}
コード例 #24
0
ファイル: SiconosVector.cpp プロジェクト: siconos/siconos
void SiconosVector::subBlock(unsigned int index, const SiconosVector& vIn)
{
  // Add vIn from the current vector, starting from position "index".
  // vIn may be a BlockVector.

  //  if ( num != 1 ) SiconosVectorException::selfThrow("SiconosVector::subBlock : vector should be dense");

  unsigned int end = vIn.size();
  if ((index + end) > size()) SiconosVectorException::selfThrow("SiconosVector::subBlock : invalid ranges");

  unsigned int numVin = vIn.num();
  if (numVin != num()) SiconosVectorException::selfThrow("SiconosVector::subBlock : inconsistent types.");

  if (_dense)
    noalias(ublas::subrange(*vect.Dense, index, index + end)) -= *vIn.dense();
  else
    noalias(ublas::subrange(*vect.Sparse, index, index + end)) -= *vIn.sparse();
}
コード例 #25
0
/*y = h(X)*/
void NonlinearRelationWithSign::computeh(double t, SiconosVector& x, SiconosVector& lambda, SiconosVector& y)
{

  //SiconosVector& lambda = *inter.lambda(0);

#ifdef SICONOS_DEBUG
  std::cout << "******** NonlinearRelationWithSign::computeh computeh at " << t << std::endl;
#endif


  y.setValue(0, 4.0 - x(0));
  y.setValue(1, 4.0 - x(1));
  y.setValue(2, 8.0 - x(0));
  y.setValue(3, 8.0 - x(1));
#ifdef SICONOS_DEBUG
  std::cout << "modif heval : \n";
  y.display();
#endif
}
コード例 #26
0
ファイル: SimpleMatrix.cpp プロジェクト: fperignon/siconos
// x block, y siconos
void private_prod(const SiconosMatrix& A, unsigned int startRow, const SiconosVector& x, SiconosVector& y, bool init)
{
  assert(!(A.isPLUFactorized()) && "A is PLUFactorized in prod !!");

  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of A, between el. of index (row) startRow and startRow + sizeY

  if (init) // y = subA * x , else y += subA * x
    y.zero();
  private_addprod(A, startRow, 0, x, y);
}
コード例 #27
0
ファイル: SiconosVector.cpp プロジェクト: siconos/siconos
void SiconosVector::setBlock(unsigned int index, const SiconosVector& vIn)
{
  // Set current vector elements, starting from position "index", to the values of vector vIn

  // Exceptions ...
  assert(&vIn != this && "SiconosVector::this->setBlock(pos,vIn): vIn = this.");

  assert(index < size() && "SiconosVector::setBlock : invalid ranges");

  unsigned int end = vIn.size() + index;
  assert(end <= size() && "SiconosVector::setBlock : invalid ranges");

  assert (vIn.num() == num() && "SiconosVector::setBlock: inconsistent types.");

  if (_dense)
    noalias(ublas::subrange(*vect.Dense, index, end)) = *vIn.dense();
  else
    noalias(ublas::subrange(*vect.Sparse, index, end)) = *vIn.sparse();
}
コード例 #28
0
ファイル: SiconosVector.cpp プロジェクト: siconos/siconos
SiconosVector::SiconosVector(const SiconosVector& v1, const SiconosVector& v2)
{
  unsigned int size1 = v1.size();
  if (ask<IsDense>(v1) && ask<IsDense>(v2))
  {
    _dense = true;
    vect.Dense = new DenseVect(size1 + v2.size());
  }
  else if (ask<IsSparse>(v1) && ask<IsSparse>(v2))
  {
    _dense = false;
    vect.Sparse = new SparseVect(size1 + v2.size());
  }
  else
  {
    SiconosVectorException::selfThrow("SiconosVector::SiconosVector :: mixed dense and sparse vector detected");
  }
  setBlock(0, v1);
  setBlock(size1, v2);
}
コード例 #29
0
ファイル: BulletR.cpp プロジェクト: radarsat1/siconos
void BulletR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  DEBUG_BEGIN("BulletR::computeh(...)\n");

  NewtonEulerR::computeh(time, q0, y);

  DEBUG_PRINT("start of computeh\n");

  btVector3 posa = _contactPoints->getPositionWorldOnA();
  btVector3 posb = _contactPoints->getPositionWorldOnB();
  if (_flip) {
      posa = _contactPoints->getPositionWorldOnB();
      posb = _contactPoints->getPositionWorldOnA();
  }

  (*pc1())(0) = posa[0];
  (*pc1())(1) = posa[1];
  (*pc1())(2) = posa[2];
  (*pc2())(0) = posb[0];
  (*pc2())(1) = posb[1];
  (*pc2())(2) = posb[2];

  {
    y.setValue(0, _contactPoints->getDistance());

    (*nc())(0) = _contactPoints->m_normalWorldOnB[0] * (_flip ? -1 : 1);
    (*nc())(1) = _contactPoints->m_normalWorldOnB[1] * (_flip ? -1 : 1);
    (*nc())(2) = _contactPoints->m_normalWorldOnB[2] * (_flip ? -1 : 1);
  }

  DEBUG_PRINTF("distance : %g\n",  y.getValue(0));


  DEBUG_PRINTF("position on A : %g,%g,%g\n", posa[0], posa[1], posa[2]);
  DEBUG_PRINTF("position on B : %g,%g,%g\n", posb[0], posb[1], posb[2]);
  DEBUG_PRINTF("normal on B   : %g,%g,%g\n", (*nc())(0), (*nc())(1), (*nc())(2));

  DEBUG_END("BulletR::computeh(...)\n");


}
コード例 #30
0
void LagrangianRheonomousR::computeJachq(double time,  SiconosVector& q, SiconosVector& z)
{
  // Note that second input arg is useless.
  if (_pluginJachq->fPtr)
  {
    // Warning: temporary method to have contiguous values in
    // memory, copy of block to simple.
    ((FPtr4)(_pluginJachq->fPtr))(q.size(), &(q)(0), time, _jachq->size(0), &(*_jachq)(0, 0), z.size(), &(z)(0));
    // Copy data that might have been changed in the plug-in call.
  }
  // else nothing.
}