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 LagrangianRheonomousR::computeOutput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int derivativeNumber)
{
VectorOfBlockVectors& DSlink = *interProp.DSlink;
SiconosVector q = *DSlink[LagrangianR::q0];
SiconosVector z = *DSlink[LagrangianR::z];
SiconosVector& y = *inter.y(derivativeNumber);
if (derivativeNumber == 0)
computeh(time, q, z, y);
else
{
computeJachq(time, q, z);
if (derivativeNumber == 1)
{
// Computation of the partial derivative w.r.t time of h(q,t)
computehDot(time, q, z);
// Computation of the partial derivative w.r.t q of h(q,t) : \nabla_q h(q,t) \dot q
prod(*_jachq, *DSlink[LagrangianR::q1], y);
// Sum of the terms
y += *_hDot;
}
else if (derivativeNumber == 2)
prod(*_jachq, *DSlink[LagrangianR::q2], y); // Approx:, ...
// \warning : the computation of y[2] (in event-driven
// simulation for instance) is approximated by y[2] =
// Jach[0]q[2]. For the moment, other terms are neglected
// (especially, partial derivatives with respect to time).
else
RuntimeException::selfThrow("LagrangianRheonomousR::computeOutput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int derivativeNumber) index >2 not yet implemented.");
}
*DSlink[LagrangianR::z] = z;
}
void NewtonEulerR::computeOutput(double time, Interaction& inter, unsigned int derivativeNumber)
{
DEBUG_BEGIN("NewtonEulerR::computeOutput(...)\n");
DEBUG_PRINTF("with time = %f and derivativeNumber = %i starts\n", time, derivativeNumber);
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
SiconosVector& y = *inter.y(derivativeNumber);
BlockVector& q = *DSlink[NewtonEulerR::q0];
if (derivativeNumber == 0)
{
computeh(time, q, y);
}
else
{
/* \warning V.A. 15/04/2016
* We decide finally not to update the Jacobian there. To be discussed
*/
// computeJachq(time, inter, DSlink[NewtonEulerR::q0]);
// computeJachqT(inter, DSlink[NewtonEulerR::q0]);
if (derivativeNumber == 1)
{
assert(_jachqT);
assert(DSlink[NewtonEulerR::velocity]);
DEBUG_EXPR(_jachqT->display();); DEBUG_EXPR((*DSlink[NewtonEulerR::velocity]).display(););
virtual void computeOutput(double t, Interaction& inter, InteractionProperties& interProp, unsigned int derivativeNumber)
{
VectorOfBlockVectors& DSlink = *interProp.DSlink;
if (derivativeNumber == 0)
{
computeh(t, *DSlink[NewtonEulerR::q0], *inter.y(0));
}
else
{
R_CLASS::computeOutput(t, inter, interProp, derivativeNumber);
}
}
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 FirstOrderType1R::computeOutput(double time, Interaction& inter, InteractionProperties& interProp, unsigned int level)
{
SiconosVector& y = *inter.y(0);
// Warning: temporary method to have contiguous values in memory, copy of block to simple.
VectorOfBlockVectors& DSlink = *interProp.DSlink;
VectorOfVectors& workV = *interProp.workVectors;
SiconosVector& workX = *workV[FirstOrderR::vec_x];
workX = *DSlink[FirstOrderR::x];
SiconosVector& workZ = *workV[FirstOrderR::vec_z];
workZ = *DSlink[FirstOrderR::z];
computeh(time, workX, workZ, y);
*DSlink[FirstOrderR::z] = workZ;
}
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 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);
}
}
