C++ SiconosVector::setValue方法代码示例

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);
};

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();
}

  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;
  }

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);
}

/*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
}

/*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
}

void KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  DEBUG_BEGIN("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
  DEBUG_EXPR(q0.display());

  double X1 = q0.getValue(0);
  double Y1 = q0.getValue(1);
  double Z1 = q0.getValue(2);
  double q10 = q0.getValue(3);
  double q11 = q0.getValue(4);
  double q12 = q0.getValue(5);
  double q13 = q0.getValue(6);
  DEBUG_PRINTF("X1 = %12.8e,\t Y1 = %12.8e,\t Z1 = %12.8e,\n",X1,Y1,Z1);
  DEBUG_PRINTF("q10 = %12.8e,\t q11 = %12.8e,\t q12 = %12.8e,\t q13 = %12.8e,\n",q10,q11,q12,q13);
  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(q0.getNumberOfBlocks()>1)
  {
    // SP::SiconosVector x2 = _d2->q();
    // DEBUG_EXPR( _d2->q()->display(););
    X2 = q0.getValue(7);
    Y2 = q0.getValue(8);
    Z2 = q0.getValue(9);
    q20 = q0.getValue(10);
    q21 = q0.getValue(11);
    q22 = q0.getValue(12);
    q23 = q0.getValue(13);
  }
  y.setValue(0, Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(1, Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(2, Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  DEBUG_EXPR(y.display());
  DEBUG_END("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
    
}

void OccR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  const OccContactShape& pcsh1 = *this->_contact1.contactShape();
  const OccContactShape& pcsh2 = *this->_contact2.contactShape();

  SP::ContactShapeDistance pdist = _geometer->distance(pcsh1, pcsh2);
  ContactShapeDistance& dist = *pdist;

  double& X1 = dist.x1;
  double& Y1 = dist.y1;
  double& Z1 = dist.z1;

  double& X2 = dist.x2;
  double& Y2 = dist.y2;
  double& Z2 = dist.z2;

  double& n1x = dist.nx;
  double& n1y = dist.ny;
  double& n1z = dist.nz;

  if(_offsetp1)
  {
    _Pc1->setValue(0, X1+_offset*n1x);
    _Pc1->setValue(1, Y1+_offset*n1y);
    _Pc1->setValue(2, Z1+_offset*n1z);
    _Pc2->setValue(0, X2);
    _Pc2->setValue(1, Y2);
    _Pc2->setValue(2, Z2);
  }
  else
  {
    _Pc1->setValue(0, X1);
    _Pc1->setValue(1, Y1);
    _Pc1->setValue(2, Z1);
    _Pc2->setValue(0, X2-_offset*n1x);
    _Pc2->setValue(1, Y2-_offset*n1y);
    _Pc2->setValue(2, Z2-_offset*n1z);
  }

  /* cf comments from O. Bonnefon */
  _Nc->setValue(0, -n1x);
  _Nc->setValue(1, -n1y);
  _Nc->setValue(2, -n1z);

  dist.value -= _offset;

  y.setValue(0, dist.value);

}

void BulletFrom1DLocalFrameR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  y.setValue(0, _contactPoints->getDistance());
  btVector3 posa = _contactPoints->getPositionWorldOnA();
  btVector3 posb = _contactPoints->getPositionWorldOnB();
  (*pc1())(0) = posa[0];
  (*pc1())(1) = posa[1];
  (*pc1())(2) = posa[2];
  (*pc2())(0) = posb[0];
  (*pc2())(1) = posb[1];
  (*pc2())(2) = posb[2];

  (*nc())(0) = _contactPoints->m_normalWorldOnB[0];
  (*nc())(1) = _contactPoints->m_normalWorldOnB[1];
  (*nc())(2) = _contactPoints->m_normalWorldOnB[2];;

}

void SphereNEDSPlanR::computeh(double time, BlockVector& q0, SiconosVector& y)
{

  double q_0 = q0(0);
  double q_1 = q0(1);
  double q_2 = q0(2);

  y.setValue(0, distance(q_0, q_1, q_2, r));
  _Pc1->setValue(0, q_0 - r * n1);
  _Pc1->setValue(1, q_1 - r * n2);
  _Pc1->setValue(2, q_2 - r * n3);
  _Pc2->setValue(0, q_0 - r * n1);
  _Pc2->setValue(1, q_1 - r * n2);
  _Pc2->setValue(2, q_2 - r * n3);
  _Nc->setValue(0, n1);
  _Nc->setValue(1, n2);
  _Nc->setValue(2, n3);
}

  void computeh(double time, BlockVector& q0, SiconosVector& y)
  {
    double height = fabs(q0.getValue(0)) - _sBallRadius;
    // 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, height);
    _Pc1->setValue(1, q0.getValue(1));
    _Pc1->setValue(2, q0.getValue(2));

    //_Pc2->setValue(0,hpc);
    //_Pc2->setValue(1,data[q0]->getValue(1));
    //_Pc2->setValue(2,data[q0]->getValue(2));
    //printf("my_NewtonEulerR N, Pc\n");
    //_Nc->display();
    //_Pc1->display();
  }

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");


}

/*g=g(lambda)*/
void NonlinearRelationWithSignInversed::computeg(double t, SiconosVector& lambda, SiconosVector& r)
{

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


  r.setValue(0, 10.0 * (1 + lambda(2)) * (1 - lambda(1)));
  r.setValue(1, 10.0 * (1 - lambda(0)) * (1 + lambda(3)));

#ifdef SICONOS_DEBUG
  std::cout << "NonlinearRelationWithSignInversed::computeg with lambda=" << std::endl;
  lambda.display();
  std::cout << std::endl;
  std::cout << "NonlinearRelationWithSignInversed::computeg modif g_alpha : \n";
  r.display();
  std::cout << std::endl;
#endif

}