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);
}
}
