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 }