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 FirstOrderType1R::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
// Check if an Interaction is connected to the Relation.
unsigned int sizeY = inter.getSizeOfY();
unsigned int sizeDS = inter.getSizeOfDS();
unsigned int sizeZ = DSlink[FirstOrderR::z]->size();
workV.resize(FirstOrderR::workVecSize);
workV[FirstOrderR::vec_z].reset(new SiconosVector(sizeZ));
workV[FirstOrderR::vec_x].reset(new SiconosVector(sizeDS));
workV[FirstOrderR::vec_r].reset(new SiconosVector(sizeDS));
workM.resize(FirstOrderR::mat_workMatSize);
if (!_C)
workM[FirstOrderR::mat_C].reset(new SimpleMatrix(sizeY, sizeDS));
if (!_D)
workM[FirstOrderR::mat_D].reset(new SimpleMatrix(sizeY, sizeY));
if (!_F)
workM[FirstOrderR::mat_F].reset(new SimpleMatrix(sizeY, sizeZ));
if (!_B)
workM[FirstOrderR::mat_B].reset(new SimpleMatrix(sizeDS, sizeY));
}
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);
};
Spectrum DiffuseAreaLight::Sample_Le(const Point2f &u1, const Point2f &u2,
Float time, Ray *ray, Normal3f *nLight,
Float *pdfPos, Float *pdfDir) const {
ProfilePhase _(Prof::LightSample);
// Sample a point on the area light's _Shape_, _pShape_
Interaction pShape = shape->Sample(u1, pdfPos);
pShape.mediumInterface = mediumInterface;
*nLight = pShape.n;
// Sample a cosine-weighted outgoing direction _w_ for area light
Vector3f w;
if (twoSided) {
Point2f u = u2;
// Choose a side to sample and then remap u[0] to [0,1] before
// applying cosine-weighted hemisphere sampling for the chosen side.
if (u[0] < .5) {
u[0] = std::min(u[0] * 2, OneMinusEpsilon);
w = CosineSampleHemisphere(u);
} else {
u[0] = std::min((u[0] - .5f) * 2, OneMinusEpsilon);
w = CosineSampleHemisphere(u);
w.z *= -1;
}
*pdfDir = 0.5f * CosineHemispherePdf(std::abs(w.z));
} else {
w = CosineSampleHemisphere(u2);
*pdfDir = CosineHemispherePdf(w.z);
}
Vector3f v1, v2, n(pShape.n);
CoordinateSystem(n, &v1, &v2);
w = w.x * v1 + w.y * v2 + w.z * n;
*ray = pShape.SpawnRay(w);
return L(pShape, w);
}
void FirstOrderLinearR::initialize(Interaction& inter)
{
FirstOrderR::initialize(inter);
// get interesting size
unsigned int sizeY = inter.dimension();
unsigned int sizeX = inter.getSizeOfDS();
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
unsigned int sizeZ = DSlink[FirstOrderR::z]->size();
VectorOfSMatrices& relationMat = inter.relationMatrices();
VectorOfVectors & relationVec= inter.relationVectors();
if (!_C && _pluginJachx->fPtr)
relationMat[FirstOrderR::mat_C].reset(new SimpleMatrix(sizeY, sizeX));
if (!_D && _pluginJachlambda->fPtr)
relationMat[FirstOrderR::mat_D].reset(new SimpleMatrix(sizeY, sizeY));
if (!_B && _pluginJacglambda->fPtr)
relationMat[FirstOrderR::mat_B].reset(new SimpleMatrix(sizeX, sizeY));
if (!_F && _pluginf->fPtr)
relationMat[FirstOrderR::mat_F].reset(new SimpleMatrix(sizeY, sizeZ));
if (!_e && _plugine->fPtr)
relationVec[FirstOrderR::e].reset(new SiconosVector(sizeY));
checkSize(inter);
}
void InteractionBeginHomerManipulationActionEvent::execute() {
int i;
unsigned short type, id;
User* user;
bool localPipe = false;
HomerManipulationActionModel* manipulationModel = NULL;
user = UserDatabase::getUserById(userId);
assert(user);
if (userId == UserDatabase::getLocalUserId())
localPipe = true;
if (localPipe) {
Interaction* interactionModule = (Interaction*)SystemCore::getModuleByName("Interaction");
assert(interactionModule);
manipulationModel
= dynamic_cast<HomerManipulationActionModel*> (interactionModule->getManipulationActionModel());
} // if
for (i = 0; i < (int)manipulatingEntityIds.size(); i++) {
split(manipulatingEntityIds[i], type, id);
// printd("Opening Pipe with type %i and id %i\n", type, id);
TransformationPipe* manipulationPipe = TransformationManager::openPipe(
INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 1, 0, type, id, 0, !localPipe, user);
if (manipulationModel)
manipulationModel->setManipulationPipe(manipulatingEntityIds[i], manipulationPipe);
} // for
}
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(););
void insert_interaction(unordered_map<string,Interaction>& map,Interaction inter,
vector<Ht_matrix> const& matrices,vector<double> const& theta, vector<Dataset> const& all_datasets){
string repres = inter.as_string();
if(map.count(repres) > 0) // element already in map, do nothing
return;
if(inter.check_for_map(matrices,theta,all_datasets))
map[repres] = inter;
}
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 FirstOrderLinearR::checkSize(Interaction& inter)
{
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
// get interesting size
unsigned int sizeY = inter.dimension();
unsigned int sizeX = inter.getSizeOfDS();
unsigned int sizeZ = DSlink[FirstOrderR::z]->size();
// Check if various operators sizes are consistent.
// Reference: interaction.
if (_C)
{
if (_C->size(0) == 0)
_C->resize(sizeX, sizeY);
else
assert((_C->size(0) == sizeY && _C->size(1) == sizeX) && "FirstOrderLinearR::initialize , inconsistent size between C and Interaction.");
}
if (_B)
{
if (_B->size(0) == 0)
_B->resize(sizeY, sizeX);
else
assert((_B->size(1) == sizeY && _B->size(0) == sizeX) && "FirstOrderLinearR::initialize , inconsistent size between B and interaction.");
}
// C and B are the minimum inputs. The others may remain null.
if (_D)
{
if (_D->size(0) == 0)
_D->resize(sizeY, sizeY);
else
assert((_D->size(0) == sizeY || _D->size(1) == sizeY) && "FirstOrderLinearR::initialize , inconsistent size between C and D.");
}
if (_F)
{
if (_F->size(0) == 0)
_F->resize(sizeY, sizeZ);
else
assert(((_F->size(0) == sizeY) && (_F->size(1) == sizeZ)) && "FirstOrderLinearR::initialize , inconsistent size between C and F.");
}
if (_e)
{
if (_e->size() == 0)
_e->resize(sizeY);
else
assert(_e->size() == sizeY && "FirstOrderLinearR::initialize , inconsistent size between C and e.");
}
}
void PhysicsEndSpringManipulationActionEvent::execute()
{
int i;
unsigned short type, id;
User* user;
bool localPipe = false;
TransformationPipe* manipulationPipe;
PhysicsSpringManipulationActionModel* manipulationModel = NULL;
user = UserDatabase::getUserById(userId);
assert(user);
if (userId == UserDatabase::getLocalUserId())
localPipe = true;
if (localPipe)
{
Interaction* interactionModule = (Interaction*)SystemCore::getModuleByName("Interaction");
assert(interactionModule);
manipulationModel = dynamic_cast<PhysicsSpringManipulationActionModel*>(interactionModule->getManipulationActionModel());
} // if
/* DEPRECATED: PIPE FOR PHYSICSENTITY IS MANAGED BY PHYSICS MODULE
for (i=0; i < (int)physicsEntityIds.size(); i++)
{
split(physicsEntityIds[i], type, id);
manipulationPipe = TransformationManager::getPipe(INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 2, 0, type, id, 0, !localPipe, user);
if (!manipulationPipe)
printd(WARNING, "PhysicsEndSpringManipulationActionEvent::execute(): could not find manipulation Pipe for user with ID %u\n", userId);
else
TransformationManager::closePipe(manipulationPipe);
if (manipulationModel)
manipulationModel->clearPhysicsEntityPipe(physicsEntityIds[i]);
} // for
*/
for (i=0; i < (int)nonPhysicsEntityIds.size(); i++)
{
split(nonPhysicsEntityIds[i], type, id);
manipulationPipe = TransformationManager::getPipe(INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 1, 0, type, id, 0, !localPipe, user);
if (!manipulationPipe)
printd(WARNING, "PhysicsEndSpringManipulationActionEvent::execute(): could not find manipulation Pipe for user with ID %u\n", userId);
else
TransformationManager::closePipe(manipulationPipe);
if (manipulationModel)
manipulationModel->clearNonPhysicsEntityPipe(nonPhysicsEntityIds[i]);
} // for
} // execute
void KneeJointR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
NewtonEulerR::initComponents(inter, DSlink, workV, workM);
if (!_dotjachq)
{
unsigned int sizeY = inter.getSizeOfY();
unsigned int xSize = inter.getSizeOfDS();
unsigned int qSize = 7 * (xSize / 6);
_dotjachq.reset(new SimpleMatrix(sizeY, qSize));
}
}
void InteractionHighlightSelectionActionEvent::execute() {
int i;
Entity* entity;
ModelInterface* highlightModel;
HighlightSelectionActionModel* selectionModel = NULL;
// std::string modelPath = Configuration::getPath("Highlighters");
SceneGraphInterface* sgIF = OutputInterface::getSceneGraphInterface();
if (!sgIF) {
printd(ERROR,
"InteractionHighlightSelectionActionEvent::execute(): no SceneGraphInterface found!\n");
return;
} // if
// modelPath = modelPath + modelUrl;
std::string modelPath = getConcatenatedPath(modelUrl, "Highlighters");
highlightModel = sgIF->loadModel(modelType, modelPath);
if (!highlightModel) {
printd(
ERROR,
"InteractionHighlightSelectionActionEvent::execute(): failed to load highlightmodel %s\n",
modelPath.c_str());
return;
} // if
if (userId == UserDatabase::getLocalUserId()) {
Interaction* interactionModule = (Interaction*)SystemCore::getModuleByName("Interaction");
assert(interactionModule);
selectionModel
= dynamic_cast<HighlightSelectionActionModel*> (interactionModule->getSelectionActionModel());
} // if
for (i = 0; i < (int)highlightEntityIds.size(); i++) {
entity = WorldDatabase::getEntityWithTypeInstanceId(highlightEntityIds[i]);
if (entity)
highlightEntity(entity, sgIF, highlightModel, selectionModel);
else
printd(WARNING,
"InteractionHighlightSelectionActionEvent::execute(): Could not find Entity for highlighting!\n");
// highlightEntity(highlightEntityIds[i], sgIF, highlightModel, selectionModel)
} // for
for (i = 0; i < (int)unhighlightEntityIds.size(); i++) {
entity = WorldDatabase::getEntityWithTypeInstanceId(unhighlightEntityIds[i]);
if (entity)
unhighlightEntity(entity, sgIF, selectionModel);
else
printd(WARNING,
"InteractionHighlightSelectionActionEvent::execute(): Could not find Entity for unhighlighting!\n");
// unhighlightEntity(unhighlightEntityIds[i], sgIF, highlightModel, selectionModel)
} // for
} // execute
void NewtonEulerR::initialize(Interaction& inter)
{
DEBUG_BEGIN("NewtonEulerR::initialize(Interaction& inter)\n");
unsigned int ySize = inter.dimension();
unsigned int xSize = inter.getSizeOfDS();
unsigned int qSize = 7 * (xSize / 6);
if (!_jachq)
_jachq.reset(new SimpleMatrix(ySize, qSize));
else
{
if (_jachq->size(0) == 0)
{
// if the matrix dim are null
_jachq->resize(ySize, qSize);
}
else
{
assert((_jachq->size(1) == qSize && _jachq->size(0) == ySize) ||
(printf("NewtonEuler::initializeWorkVectorsAndMatrices _jachq->size(1) = %d ,_qsize = %d , _jachq->size(0) = %d ,_ysize =%d \n", _jachq->size(1), qSize, _jachq->size(0), ySize) && false) ||
("NewtonEuler::initializeWorkVectorsAndMatrices inconsistent sizes between _jachq matrix and the interaction." && false));
}
}
DEBUG_EXPR(_jachq->display());
if (! _jachqT)
_jachqT.reset(new SimpleMatrix(ySize, xSize));
if (! _T)
{
_T.reset(new SimpleMatrix(7, 6));
_T->zero();
_T->setValue(0, 0, 1.0);
_T->setValue(1, 1, 1.0);
_T->setValue(2, 2, 1.0);
}
DEBUG_EXPR(_jachqT->display());
VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
if (!_contactForce)
{
_contactForce.reset(new SiconosVector(DSlink[NewtonEulerR::p1]->size()));
_contactForce->zero();
}
DEBUG_END("NewtonEulerR::initialize(Interaction& inter)\n");
}
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;
}
bool Sphere::Sample(const Interaction &ref, const Point2f &sample,
Interaction *it) const {
// Compute coordinate system for sphere sampling
Point3f pCenter = (*ObjectToWorld)(Point3f(0, 0, 0));
Point3f pOrigin =
OffsetRayOrigin(ref.p, ref.pError, ref.n, pCenter - ref.p);
Vector3f wc = Normalize(pCenter - ref.p);
Vector3f wcX, wcY;
CoordinateSystem(wc, &wcX, &wcY);
// Sample uniformly on sphere if $\pt{}$ is inside it
if (DistanceSquared(pOrigin, pCenter) <= 1.0001f * radius * radius)
return Sample(sample, it);
// Sample sphere uniformly inside subtended cone
Float sinThetaMax2 = radius * radius / DistanceSquared(ref.p, pCenter);
Float cosThetaMax =
std::sqrt(std::max((Float)0., (Float)1. - sinThetaMax2));
SurfaceInteraction isectSphere;
Float tHit;
Ray r = ref.SpawnRay(UniformSampleCone(sample, cosThetaMax, wcX, wcY, wc));
if (!Intersect(r, &tHit, &isectSphere)) return false;
*it = isectSphere;
if (ReverseOrientation) it->n *= -1.f;
return true;
}
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;
}
Spectrum DiffuseAreaLight::Sample_Le(const Point2f &u1, const Point2f &u2,
Float time, Ray *ray, Normal3f *nLight,
Float *pdfPos, Float *pdfDir) const {
Interaction pShape = shape->Sample(u1);
pShape.mediumInterface = mediumInterface;
Vector3f w = CosineSampleHemisphere(u2);
*pdfDir = CosineHemispherePdf(w.z);
// Transform cosine-weighted direction to normal's coordinate system
Vector3f v1, v2, n(pShape.n);
CoordinateSystem(n, &v1, &v2);
w = w.x * v1 + w.y * v2 + w.z * n;
*ray = pShape.SpawnRay(w);
*nLight = pShape.n;
*pdfPos = shape->Pdf(pShape);
return L(pShape, w);
}
std::string
Interaction::
dotBar( const Interaction & i, const bool fullLength )
{
#if INTARNA_IN_DEBUG_MODE
if (!i.isValid())
throw std::runtime_error("Interaction::dotBar("+toString(i)+") not valid!");
#endif
// check whether to do full length output
if (fullLength) {
// compile dot-bar representation for full sequences
return "1"
+ std::string(i.basePairs.begin()->first, '.' ) // leading unpaired s1
+ dotSomething(i.basePairs.begin(), i.basePairs.end(), true, '|') // s1 structure
+ std::string(i.s1->size() - i.basePairs.rbegin()->first -1, '.' ) // trailing unpaired s1
+ "&"
+ "1"
+ std::string(i.basePairs.rbegin()->second, '.' ) // trailing unpaired s2
+ dotSomething(i.basePairs.rbegin(), i.basePairs.rend(), false, '|') // s2 structure
+ std::string( i.s2->size() - i.basePairs.begin()->second -1, '.' ) // leading unpaired s2
;
} else {
// compile dot-bar representation for interacting subsequences only
return toString(i.basePairs.begin()->first +1)
+ dotSomething(i.basePairs.begin(), i.basePairs.end(), true, '|')
+"&"
+toString(i.basePairs.rbegin()->second +1)
+ dotSomething(i.basePairs.rbegin(), i.basePairs.rend(), false, '|')
;
}
}
std::string
Interaction::
dotBracket( const Interaction & i, const char symOpen, const char symClose, const bool fullLength )
{
#if INTARNA_IN_DEBUG_MODE
if (!i.isValid())
throw std::runtime_error("Interaction::dotBracket("+toString(i)+") not valid!");
#endif
if (fullLength) {
// compile dot-bracket representation for full sequence lengths
return std::string(i.basePairs.begin()->first, '.' ) // leading unpaired s1
+ dotSomething(i.basePairs.begin(), i.basePairs.end(), true, symOpen) // s1 structure
+ std::string(i.s1->size() - i.basePairs.rbegin()->first -1, '.' ) // trailing unpaired s1
+"&"
+ std::string(i.s2->size() - i.basePairs.rbegin()->second -1, '.' ) // leading unpaired s2
+ dotSomething(i.basePairs.rbegin(), i.basePairs.rend(), false, symClose) // s2 structure
+ std::string(i.basePairs.rbegin()->second, '.' ) // trailing unpaired s2
;
} else {
// compile dot-bracket representation for interacting subsequences only
return dotSomething(i.basePairs.begin(), i.basePairs.end(), true, symOpen)
+"&"
+ dotSomething(i.basePairs.rbegin(), i.basePairs.rend(), false, symClose)
;
}
}
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 PhysicsBeginSpringManipulationActionEvent::execute()
{
int i;
unsigned short type, id;
User* user;
bool localPipe = false;
PhysicsSpringManipulationActionModel* manipulationModel = NULL;
TransformationPipe* manipulationPipe;
user = UserDatabase::getUserById(userId);
assert(user);
if (userId == UserDatabase::getLocalUserId())
localPipe = true;
if (localPipe)
{
Interaction* interactionModule = (Interaction*)SystemCore::getModuleByName("Interaction");
assert(interactionModule);
manipulationModel = dynamic_cast<PhysicsSpringManipulationActionModel*>(interactionModule->getManipulationActionModel());
} // if
/* DEPRECATED: PIPE FOR PHYSICSENTITY IS MANAGED BY PHYSICS MODULE
for (i=0; i < (int)physicsEntityIds.size(); i++)
{
split(physicsEntityIds[i], type, id);
// printd("Opening Pipe with type %i and id %i\n", type, id);
// manipulationPipe = TransformationManager::openPipe(INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 2, 0, type, id, 0, !localPipe, user);
// if (manipulationModel)
// manipulationModel->setPhysicsEntityPipe(physicsEntityIds[i], manipulationPipe);
} // for
*/
for (i=0; i < (int)nonPhysicsEntityIds.size(); i++)
{
split(nonPhysicsEntityIds[i], type, id);
// printd("Opening Pipe with type %i and id %i\n", type, id);
manipulationPipe = TransformationManager::openPipe(INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 1, 0, type, id, 0, !localPipe, user);
if (manipulationModel)
manipulationModel->setNonPhysicsEntityPipe(nonPhysicsEntityIds[i], manipulationPipe);
} // for
} // execute
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 LagrangianRheonomousR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
LagrangianR::initComponents(inter, DSlink, workV, workM);
unsigned int sizeY = inter.getSizeOfY();
// hDot
if (!_hDot)
_hDot.reset(new SiconosVector(sizeY));
else
_hDot->resize(sizeY);
if (_pluginJachq->fPtr && !_jachq)
{
unsigned int sizeY = inter.getSizeOfY();
unsigned int sizeQ = DSlink[LagrangianR::q0]->size();
_jachq.reset(new SimpleMatrix(sizeY, sizeQ));
}
}
PxU32 Sc::ConstraintProjectionTree::projectionTreeBuildStep(ConstraintGroupNode& node, ConstraintSim* cToParent, ConstraintGroupNode** nodeQueue)
{
PX_ASSERT(node.readFlag(ConstraintGroupNode::eDISCOVERED));
PxU32 nodeQueueFillCount = 0;
//go through all constraints attached to the body.
BodySim* body = node.body;
PxU32 size = body->getActorInteractionCount();
Sc::Interaction** interactions = body->getActorInteractions();
while(size--)
{
Interaction* interaction = *interactions++;
if (interaction->getType() == InteractionType::eCONSTRAINTSHADER)
{
ConstraintSim* c = static_cast<ConstraintInteraction*>(interaction)->getConstraint();
if (c != cToParent) //don't go back along the edge we came from (not really necessary I guess since the ConstraintGroupNode::eDISCOVERED marker should solve this)
{
PxU32 projectToBody, projectToOtherBody;
BodySim* neighbor;
getConstraintStatus(*c, body, neighbor, projectToBody, projectToOtherBody);
if(!isFixedBody(neighbor) && (!projectToOtherBody || projectToBody)) //just ignore the eventual constraint with environment over here. Body might be attached to multiple fixed anchors.
//Also make sure to ignore one-way projection that goes the opposite way.
{
ConstraintGroupNode* neighborNode = neighbor->getConstraintGroup();
PX_ASSERT(neighborNode);
if (!neighborNode->readFlag(ConstraintGroupNode::eDISCOVERED))
{
*nodeQueue = neighborNode;
neighborNode->initProjectionData(&node, c);
neighborNode->raiseFlag(ConstraintGroupNode::eDISCOVERED); //flag body nodes that we process so we can detect loops
nodeQueueFillCount++;
nodeQueue++;
}
}
}
}
}
return nodeQueueFillCount;
}
Spectrum DiffuseAreaLight::Sample_Le(const Point2f &u1, const Point2f &u2,
Float time, Ray *ray, Normal3f *nLight,
Float *pdfPos, Float *pdfDir) const {
// Sample a point on the area light's _Shape_, _pShape_
Interaction pShape = shape->Sample(u1);
pShape.mediumInterface = mediumInterface;
*pdfPos = shape->Pdf(pShape);
*nLight = pShape.n;
// Sample a cosine-weighted outgoing direction _w_ for area light
Vector3f w = CosineSampleHemisphere(u2);
*pdfDir = CosineHemispherePdf(w.z);
Vector3f v1, v2, n(pShape.n);
CoordinateSystem(n, &v1, &v2);
w = w.x * v1 + w.y * v2 + w.z * n;
*ray = pShape.SpawnRay(w);
return L(pShape, w);
}
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);
}
unsigned int OSNSMatrix::getPositionOfInteractionBlock(Interaction& inter) const
{
// Note FP: I think the return value below is not the right one :
// this position does not depend on the interaction but on
// the OSNS and the corresponding indexSet.
// One Interaction may have different absolute positions if it is present
// in several OSNS. ==> add this pos as a property on vertex in Interactions Graph
//
return inter.absolutePosition();
}
void LagrangianCompliantR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
LagrangianR::initComponents(inter, DSlink, workV, workM);
unsigned int sizeY = inter.getSizeOfY();
if (! _jachlambda)
_jachlambda.reset(new SimpleMatrix(sizeY, sizeY));
else
_jachlambda->resize(sizeY, sizeY);
}
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom3DLocalFrameR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
NewtonEulerFrom1DLocalFrameR::initComponents(inter, DSlink, workV, workM);
unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
/*keep only the distance.*/
_jachq.reset(new SimpleMatrix(3, qSize));
_Mabs_C.reset(new SimpleMatrix(3, 3));
_AUX2.reset(new SimpleMatrix(3, 3));
// _isContact=1;
}
