void LagrangianCompliantR::computeOutput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int derivativeNumber) { VectorOfBlockVectors& DSlink = *interProp.DSlink; SiconosVector workZ = *DSlink[LagrangianR::z]; if (derivativeNumber == 0) { SiconosVector& y = *inter.y(0); SiconosVector& lambda = *inter.lambda(0); SiconosVector workQ = *DSlink[LagrangianR::q0]; computeh(time, workQ, lambda, workZ, y); } else { SiconosVector& y = *inter.y(derivativeNumber); SiconosVector& lambda = *inter.lambda(derivativeNumber); SiconosVector workQ = *DSlink[LagrangianR::q0]; computeJachq(time, workQ, lambda, workZ); computeJachlambda(time, workQ, lambda, workZ); if (derivativeNumber == 1) { // y = Jach[0] q1 + Jach[1] lambda prod(*_jachq, *DSlink[LagrangianR::q1], y); prod(*_jachlambda, lambda, y, false); } else if (derivativeNumber == 2) prod(*_jachq, *DSlink[LagrangianR::q2], y); // Approx: y[2] = Jach[0]q[2], other terms are neglected ... else RuntimeException::selfThrow("LagrangianCompliantR::computeOutput, index out of range or not yet implemented."); } *DSlink[LagrangianR::z] = workZ; }
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 LagrangianLinearTIR::computeInput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int level) { // get lambda of the concerned interaction SiconosVector& lambda = *inter.lambda(level); VectorOfBlockVectors& DSlink = *interProp.DSlink; // computation of p = Ht lambda prod(lambda, *_jachq, *DSlink[LagrangianR::p0 + level], false); }
void LagrangianCompliantR::computeJach(double time, Interaction& inter, InteractionProperties& interProp) { VectorOfBlockVectors& DSlink = *interProp.DSlink; SiconosVector q = *DSlink[LagrangianR::q0]; SiconosVector z = *DSlink[LagrangianR::z]; SiconosVector& lambda = *inter.lambda(0); computeJachq(time, q, lambda, z); computeJachlambda(time, q, lambda, z); }
void FirstOrderLinearR::computeInput(double time, Interaction& inter, unsigned int level) { SiconosVector& lambda = *inter.lambda(level); VectorOfBlockVectors& DSlink = inter.linkToDSVariables(); BlockVector& z = *DSlink[FirstOrderR::z]; SP::SiconosVector z_vec(new SiconosVector(z)); computeg(time, lambda, *z_vec, *DSlink[FirstOrderR::r]); *DSlink[FirstOrderR::z] = *z_vec; }
void LagrangianRheonomousR::computeInput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int level) { VectorOfBlockVectors& DSlink = *interProp.DSlink; SiconosVector q = *DSlink[LagrangianR::q0]; SiconosVector z = *DSlink[LagrangianR::z]; computeJachq(time, q, z); // get lambda of the concerned interaction SiconosVector& lambda = *inter.lambda(level); // data[name] += trans(G) * lambda prod(lambda, *_jachq, *DSlink[LagrangianR::p0 + level], false); *DSlink[LagrangianR::z] = z; }
void LagrangianCompliantR::computeInput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int level) { // get lambda of the concerned interaction SiconosVector& lambda = *inter.lambda(level); VectorOfBlockVectors& DSlink = *interProp.DSlink; SiconosVector workQ = *DSlink[LagrangianR::q0]; SiconosVector workZ = *DSlink[LagrangianR::z]; computeJachq(time, workQ, lambda, workZ); // data[name] += trans(G) * lambda prod(lambda, *_jachq, *DSlink[LagrangianR::p0 + level], false); *DSlink[LagrangianR::z] = workZ; }
void FirstOrderType1R::computeJacg(double time, Interaction& inter, InteractionProperties& interProp) { VectorOfBlockVectors& DSlink = *interProp.DSlink; VectorOfVectors& workV = *interProp.workVectors; VectorOfSMatrices& workM = *interProp.workMatrices; SiconosVector& z = *workV[FirstOrderR::vec_z]; z = *DSlink[FirstOrderR::z]; if (!_B) { computeJacglambda(time, *inter.lambda(0), z, *workM[FirstOrderR::mat_B]); } *DSlink[FirstOrderR::z] = z; }
void FirstOrderLinearR::computeInput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int level) { SiconosVector& lambda = *inter.lambda(level); VectorOfBlockVectors& DSlink = *interProp.DSlink; VectorOfVectors& workV = *interProp.workVectors; VectorOfSMatrices& workM = *interProp.workMatrices; SiconosVector& z = *workV[FirstOrderR::z]; z = *DSlink[FirstOrderR::z]; computeg(time, workM, lambda, z, *DSlink[FirstOrderR::r]); *DSlink[FirstOrderR::z] = z; }
void FirstOrderLinearR::computeOutput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int level) { VectorOfBlockVectors& DSlink = *interProp.DSlink; VectorOfVectors& workV = *interProp.workVectors; VectorOfSMatrices& workM = *interProp.workMatrices; SiconosVector& z = *workV[FirstOrderR::vec_z]; z = *DSlink[FirstOrderR::z]; // We get y and lambda of the interaction (pointers) SiconosVector& y = *inter.y(0); SiconosVector& lambda = *inter.lambda(0); computeh(time, workV, workM, *DSlink[FirstOrderR::x], lambda, z, y); *DSlink[FirstOrderR::z] = z; }
void FirstOrderLinearR::computeOutput(double time, Interaction& inter, unsigned int level) { DEBUG_BEGIN("FirstOrderLinearR::computeOutput \n"); VectorOfBlockVectors& DSlink = inter.linkToDSVariables(); BlockVector& z = *DSlink[FirstOrderR::z]; BlockVector& x = *DSlink[FirstOrderR::x]; SP::SiconosVector z_vec(new SiconosVector(z)); SiconosVector& y = *inter.y(level); SiconosVector& lambda = *inter.lambda(level); computeh(time, x, lambda, *z_vec, y); *DSlink[FirstOrderR::z] = *z_vec; DEBUG_END("FirstOrderLinearR::computeOutput \n"); }
void FirstOrderType1R::computeInput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int level) { assert(_pluging && "FirstOrderType1R::computeInput() is not linked to a plugin function"); SiconosVector& lambda = *inter.lambda(level); // Warning: temporary method to have contiguous values in memory, copy of block to simple. VectorOfBlockVectors& DSlink = *interProp.DSlink; VectorOfVectors& workV = *interProp.workVectors; SiconosVector& workR = *workV[FirstOrderR::vec_r]; workR = *DSlink[FirstOrderR::r]; SiconosVector& workZ = *workV[FirstOrderR::vec_z]; workZ = *DSlink[FirstOrderR::z]; computeg(time, lambda, workZ, workR); *DSlink[FirstOrderR::r] = workR; *DSlink[FirstOrderR::z] = workZ; }
void LagrangianLinearTIR::computeOutput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int derivativeNumber) { // get y and lambda of the interaction SiconosVector& y = *inter.y(derivativeNumber); VectorOfBlockVectors& DSlink = *interProp.DSlink; prod(*_jachq, *DSlink[LagrangianR::q0 + derivativeNumber], y); if (derivativeNumber == 0) { if (_e) y += *_e; if (_F) prod(*_F, *DSlink[LagrangianR::z], y, false); } if (_jachlambda) { SiconosVector& lambda = *inter.lambda(derivativeNumber); prod(*_jachlambda, lambda, y, false); } }