C++ (Cpp) NewtonEulerDS::MObjToAbs Examples

Example #1

File: NewtonEulerFrom3DLocalFrameR.cpp Project: bremond/siconos

void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::NewtonEulerDS d1)
{

  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = d1->q()->getValue(0);
  double G1y = d1->q()->getValue(1);
  double G1z = d1->q()->getValue(2);



#ifdef NEFC3D_DEBUG
  printf("contact normal:\n");
  _Nc->display();
  printf("contact point :\n");
  _Pc1->display();
  printf("center of mass :\n");
  d1->q()->display();
#endif

  assert(_Nc->norm2() >0.0 && "NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts. Normal vector not consistent ") ;

  double t[6];
  double * pt = t;
  orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
  pt = t;
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(1, 0, *pt);
  _Mabs_C->setValue(2, 0, *(pt + 3));
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(1, 1, *(pt + 1));
  _Mabs_C->setValue(2, 1, *(pt + 4));
  _Mabs_C->setValue(0, 2, Nz);
  _Mabs_C->setValue(1, 2, *(pt + 2));
  _Mabs_C->setValue(2, 2, *(pt + 5));
#ifdef NEFC3D_DEBUG
  printf("_Mabs_C:\n");
  _Mabs_C->display();
#endif
  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

//  d1->computeMObjToAbs();
  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();



#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mobj1_abs:");
  Mobj1_abs.display();
#endif


  prod(*_NPG1, Mobj1_abs, *_AUX1, true);
#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mobj1_abs:");
  Mobj1_abs.display();
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
  _AUX1->display();
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
  _AUX2->display();
#endif

  prod(*_Mabs_C, *_AUX1, *_AUX2, true);
#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mabs_C:");
  _Mabs_C->display();
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
  _AUX1->display();
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
  _AUX2->display();
#endif


  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));
#ifdef NEFC3D_DEBUG
  printf("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, _jahcqT:\n");
  _jachqT->display();
  SP::SimpleMatrix jaux(new SimpleMatrix(*_jachqT));
  jaux->trans();
  SP::SiconosVector v(new SiconosVector(3));
  SP::SiconosVector vRes(new SiconosVector(6));
  v->zero();
  v->setValue(0, 1);
  prod(*jaux, *v, *vRes, true);
  vRes->display();
  v->zero();
  v->setValue(1, 1);
  prod(*jaux, *v, *vRes, true);
  vRes->display();
  v->zero();
  v->setValue(2, 1);
  prod(*jaux, *v, *vRes, true);
  vRes->display();
#endif
}

Example #2

File: NewtonEulerFrom3DLocalFrameR.cpp Project: bremond/siconos

void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::NewtonEulerDS d1, SP::NewtonEulerDS d2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = d1->q()->getValue(0);
  double G1y = d1->q()->getValue(1);
  double G1z = d1->q()->getValue(2);
  double G2x = d2->q()->getValue(0);
  double G2y = d2->q()->getValue(1);
  double G2z = d2->q()->getValue(2);

  double t[6];
  double * pt = t;
  orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
  pt = t;
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(1, 0, *pt);
  _Mabs_C->setValue(2, 0, *(pt + 3));
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(1, 1, *(pt + 1));
  _Mabs_C->setValue(2, 1, *(pt + 4));
  _Mabs_C->setValue(0, 2, Nz);
  _Mabs_C->setValue(1, 2, *(pt + 2));
  _Mabs_C->setValue(2, 2, *(pt + 5));

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  _NPG2->zero();

  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;


//  d1->computeMObjToAbs();
  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();


  prod(*_NPG1, Mobj1_abs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);


  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));

//  d2->computeMObjToAbs();
  SimpleMatrix& Mobj2_abs = *d2->MObjToAbs();

  prod(*_NPG2, Mobj2_abs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj + 6, -_Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj + 6, -_AUX2->getValue(ii, jj - 3));

}