/*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 }
/*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"); }
/*y = h(X,lambda)*/ void adjointInput::computeh(double t, SiconosVector& x, SiconosVector& lambda, SiconosVector& z, SiconosVector& y) { #ifdef SICONOS_DEBUG std::cout << "******** computeH at " << t << std::endl; #endif SP::SiconosVector betatmp(new SiconosVector(2)); beta(t, x, betatmp); double betap = 2.0 * ((betatmp->getValue(0)) * x(2) + (betatmp->getValue(1)) * x(3)); y(0) = lambda(1) + betap; y(1) = 2.0 - lambda(0); #ifdef SICONOS_DEBUG std::cout << "modif heval : \n"; y.display(); #endif }