C++ Interaction类代码示例

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