Пример #1
0
int main(int argc, char** argv) {
	try {
		std::cerr << "LuxRays Simple IntersectionDevice Benchmark v" << LUXRAYS_VERSION_MAJOR << "." << LUXRAYS_VERSION_MINOR << std::endl;
		std::cerr << "Usage: " << argv[0] << std::endl;

		//--------------------------------------------------------------------------
		// Create the context
		//--------------------------------------------------------------------------

		luxrays::Context *ctx = new luxrays::Context(DebugHandler);

		// Get the list of all devices available
		std::vector<luxrays::DeviceDescription *> deviceDescs = std::vector<luxrays::DeviceDescription *>(ctx->GetAvailableDeviceDescriptions());

		// Use the first device available
		//luxrays::DeviceDescription::FilterOne(deviceDescs);

		// Use the first native C++ device available
		luxrays::DeviceDescription::Filter(luxrays::DEVICE_TYPE_NATIVE_THREAD, deviceDescs);

		// Use the first OpenCL device available
		//luxrays::DeviceDescription::Filter(luxrays::DEVICE_TYPE_OPENCL_ALL, deviceDescs);
		//luxrays::DeviceDescription::FilterOne(deviceDescs);

		// Use the first OpenCL CPU device available
		//luxrays::DeviceDescription::Filter(luxrays::DEVICE_TYPE_OPENCL_ALL, deviceDescs);
		//luxrays::OpenCLDeviceDescription::Filter(luxrays::DEVICE_TYPE_OPENCL_CPU, deviceDescs);

		// Use the first OpenCL GPU device available
		//luxrays::DeviceDescription::Filter(luxrays::DEVICE_TYPE_OPENCL_ALL, deviceDescs);
		//luxrays::OpenCLDeviceDescription::Filter(luxrays::DEVICE_TYPE_OPENCL_GPU, deviceDescs);

		// Use all GPU devices available (for virtual device)
		//luxrays::DeviceDescription::Filter(luxrays::DEVICE_TYPE_OPENCL_ALL, deviceDescs);
		//luxrays::OpenCLDeviceDescription::Filter(luxrays::DEVICE_TYPE_OPENCL_GPU, deviceDescs);
		
		if (deviceDescs.size() < 1) {
			std::cerr << "Unable to find an usable intersection device" << std::endl;
			return (EXIT_FAILURE);
		}

		// Single device
		deviceDescs.resize(1);
		std::cerr << "Selected intersection device: " << deviceDescs[0]->GetName();
		luxrays::IntersectionDevice *device = ctx->AddIntersectionDevices(deviceDescs)[0];

		// Multiple devices
		//ctx->AddVirtualIntersectionDevices(deviceDescs);
		//luxrays::IntersectionDevice *device = ctx->GetIntersectionDevice()[0];
		

		// If it is a NativeThreadIntersectionDevice, you can set the number of threads
		// to use. The default is to use one for each core available.
		//((luxrays::NativeThreadIntersectionDevice *)device)->SetThreadCount(1);

		//--------------------------------------------------------------------------
		// Build the data set
		//--------------------------------------------------------------------------

		std::cerr << "Creating a " << TRIANGLE_COUNT << " triangle data set" << std::endl;

		luxrays::RandomGenerator rnd(1u);
		luxrays::Point *verts = luxrays::TriangleMesh::AllocVerticesBuffer(TRIANGLE_COUNT * 3);
		luxrays::Triangle *tris = luxrays::TriangleMesh::AllocTrianglesBuffer(TRIANGLE_COUNT);

		for (size_t i = 0; i < TRIANGLE_COUNT; ++i) {
			luxrays::Vector v0(0.1f * rnd.floatValue(), 0.1f * rnd.floatValue(), 0.1f * rnd.floatValue());
			v0.x += 0.1f * luxrays::Sgn(v0.x);
			v0.y += 0.1f * luxrays::Sgn(v0.y);
			v0.z += 0.1f * luxrays::Sgn(v0.z);

			luxrays::Vector v1, v2;
			luxrays::CoordinateSystem(v0, &v1, &v2);
			v1 *= 0.1f;
			v2 *= 0.1f;

			size_t vIndex = i * 3;
			verts[vIndex].x = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v0.x;
			verts[vIndex].y = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v0.y;
			verts[vIndex].z = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v0.z;

			verts[vIndex + 1].x = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v1.x;
			verts[vIndex + 1].y = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v1.y;
			verts[vIndex + 1].z = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v1.z;

			verts[vIndex + 2].x = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v2.x;
			verts[vIndex + 2].y = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v2.y;
			verts[vIndex + 2].z = (SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f) + v2.z;

			tris[i].v[0] = vIndex;
			tris[i].v[1] = vIndex + 1;
			tris[i].v[2] = vIndex + 2;
		}

		luxrays::TriangleMesh *mesh = new luxrays::TriangleMesh(TRIANGLE_COUNT * 3, TRIANGLE_COUNT, verts, tris);
		luxrays::DataSet *dataSet = new luxrays::DataSet(ctx);
		dataSet->Add(mesh);
		dataSet->Preprocess();
		ctx->SetDataSet(dataSet);

		//--------------------------------------------------------------------------
		// Build the ray buffers
		//--------------------------------------------------------------------------

		std::cerr << "Creating " << RAYBUFFERS_COUNT << " ray buffers" << std::endl;

		luxrays::Ray ray;
		std::queue<luxrays::RayBuffer *> todoRayBuffers;
		std::vector<luxrays::RayBuffer *> rayBuffers;
		for (size_t i = 0; i < RAYBUFFERS_COUNT; ++i) {
			luxrays::RayBuffer *rayBuffer = device->NewRayBuffer();
			todoRayBuffers.push(rayBuffer);
			rayBuffers.push_back(rayBuffer);

			for (size_t j = 0; j < rayBuffer->GetSize(); ++j) {
				ray.o = luxrays::Point(SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f,
						SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f,
						SPACE_SIZE * rnd.floatValue() - SPACE_SIZE / 2.f);

				do {
					ray.d = luxrays::Vector(rnd.floatValue(), rnd.floatValue(), rnd.floatValue());
				} while (ray.d == luxrays::Vector(0.f, 0.f, 0.f)); // Just in case ...
				ray.d = luxrays::Normalize(ray.d);

				ray.mint = luxrays::MachineEpsilon::E(ray.o);
				ray.maxt = 1000.f;

				rayBuffer->AddRay(ray);
			}
		}

		//--------------------------------------------------------------------------
		// Run the serial benchmark. This simulate one single thread pushing
		// rays to trace.
		//--------------------------------------------------------------------------

		{
			// The number of queues used
			device->SetQueueCount(1);
			// You have to set the max. number of buffers you can push between 2 pop.
			device->SetBufferCount(RAYBUFFERS_COUNT);

			ctx->Start();

			std::cerr << "Running the serial benchmark for 15 seconds..." << std::endl;
			double tStart = luxrays::WallClockTime();
			double tLastCheck = tStart;
			double bufferDone = 0.0;
			bool done = false;
			while (!done) {
				while (todoRayBuffers.size() > 0) {
					device->PushRayBuffer(todoRayBuffers.front());
					todoRayBuffers.pop();
	
					// Check if it is time to stop
					const double tNow = luxrays::WallClockTime();
					if (tNow - tLastCheck > 1.0) {
						if (tNow - tStart > 15.0) {
							done = true;
							break;
						}
	
						std::cerr << int(tNow - tStart) << "/15secs" << std::endl;
						tLastCheck = tNow;
					}
				}
	
				todoRayBuffers.push(device->PopRayBuffer());
				bufferDone += 1.0;
			}
	
			while (todoRayBuffers.size() != RAYBUFFERS_COUNT) {
				todoRayBuffers.push(device->PopRayBuffer());
				bufferDone += 1.0;
			}
			double tStop = luxrays::WallClockTime();
			double tTime = tStop - tStart;
	
			ctx->Stop();

			std::cerr << "Test total time: " << tTime << std::endl;
			std::cerr << "Test total ray buffer count: " << int(bufferDone) << std::endl;
			std::cerr << "Test ray buffer size: " << todoRayBuffers.front()->GetRayCount() << std::endl;
			double raySec = (bufferDone * todoRayBuffers.front()->GetRayCount()) / tTime;
			if (raySec < 10000.0)
				std::cerr << "Test performance: " << std::setiosflags(std::ios::fixed) << std::setprecision(2) <<
						raySec <<" rays/sec" << std::endl;
			else if (raySec < 1000000.0)
				std::cerr << "Test performance: " << std::setiosflags(std::ios::fixed) << std::setprecision(2) <<
						(raySec / 1000.0) <<"K rays/sec" << std::endl;
			else
				std::cerr << "Test performance: " << std::setiosflags(std::ios::fixed) << std::setprecision(2) <<
						(raySec / 1000000.0) <<"M rays/sec" << std::endl;
		}

		//--------------------------------------------------------------------------
		// Run the parallel benchmark. This simulate multiple threads pushing
		// rays to trace.
		//--------------------------------------------------------------------------

		{
			// The number of queues used
			device->SetQueueCount(RAYBUFFERS_COUNT);
			// You have to set the max. number of buffers you can push between 2 pop.
			device->SetBufferCount(1);

			ctx->Start();

			std::cerr << "Running the parallel benchmark for 15 seconds..." << std::endl;
			double tStart = luxrays::WallClockTime();

			for (u_int i = 0; i < RAYBUFFERS_COUNT; ++i)
				device->PushRayBuffer(rayBuffers[i], i);

			double tLastCheck = tStart;
			double bufferDone = 0.0;
			bool done = false;
			u_int queueIndex = 0;
			while (!done) {
				device->PopRayBuffer(queueIndex);
				bufferDone += 1.0;
	
				device->PushRayBuffer(rayBuffers[queueIndex], queueIndex);
	
				// Check if it is time to stop
				const double tNow = luxrays::WallClockTime();
				if (tNow - tLastCheck > 1.0) {
					if (tNow - tStart > 15.0) {
						done = true;
						break;
					}
	
					std::cerr << int(tNow - tStart) << "/15secs" << std::endl;
					tLastCheck = tNow;
				}
	
				queueIndex = (queueIndex + 1) % RAYBUFFERS_COUNT;
			}

			for (u_int i = 0; i < RAYBUFFERS_COUNT; ++i)
				device->PopRayBuffer(i);
			bufferDone += RAYBUFFERS_COUNT;

			double tStop = luxrays::WallClockTime();
			double tTime = tStop - tStart;

			ctx->Stop();

			std::cerr << "Test total time: " << tTime << std::endl;
			std::cerr << "Test total ray buffer count: " << int(bufferDone) << std::endl;
			std::cerr << "Test ray buffer size: " << todoRayBuffers.front()->GetRayCount() << std::endl;
			double raySec = (bufferDone * todoRayBuffers.front()->GetRayCount()) / tTime;
			if (raySec < 10000.0)
				std::cerr << "Test performance: " << std::setiosflags(std::ios::fixed) << std::setprecision(2) <<
						raySec <<" rays/sec" << std::endl;
			else if (raySec < 1000000.0)
				std::cerr << "Test performance: " << std::setiosflags(std::ios::fixed) << std::setprecision(2) <<
						(raySec / 1000.0) <<"K rays/sec" << std::endl;
			else
				std::cerr << "Test performance: " << std::setiosflags(std::ios::fixed) << std::setprecision(2) <<
						(raySec / 1000000.0) <<"M rays/sec" << std::endl;
		}

		//--------------------------------------------------------------------------
		// Free everything
		//--------------------------------------------------------------------------

		while (todoRayBuffers.size() > 0) {
			delete todoRayBuffers.front();
			todoRayBuffers.pop();
		}

		delete ctx;
		delete dataSet;
		mesh->Delete();
		delete mesh;

		std::cerr << "Done." << std::endl;
#if !defined(LUXRAYS_DISABLE_OPENCL)
	} catch (cl::Error &err) {
		std::cout << "OpenCL ERROR: " << err.what() << "(" << luxrays::oclErrorString(err.err()) << ")" << std::endl;
		return EXIT_FAILURE;
#endif
	} catch (std::runtime_error &err) {
		std::cout << "RUNTIME ERROR: " << err.what() << std::endl;
		return EXIT_FAILURE;
	} catch (std::exception &err) {
		std::cout << "ERROR: " << err.what() << std::endl;
		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}
void RouteSegmentsVisual::setMessage ( const tuw_nav_msgs::RouteSegments::ConstPtr& msg ) {
    static double timeOld_;
    if ( timeOld_ == msg->header.stamp.toSec() ) {
        return;
    }
    timeOld_ = msg->header.stamp.toSec();

    startPts_.clear();
    endPts_.clear();
    centerPts_.clear();
    lines_.clear ();
    arcs_.clear ();
    for ( size_t i = 0; i < msg->segments.size(); i++ ) {
        const tuw::ros_msgs::RouteSegment &segment = (const tuw::ros_msgs::RouteSegment &) msg->segments[i];
        const geometry_msgs::Pose &p0 = segment.start;
        const geometry_msgs::Pose &pc = segment.center;
        const geometry_msgs::Pose &p1 = segment.end;

        if ( show_start_points_ ) {
            startPts_.push_back ( boost::shared_ptr<rviz::Shape> ( new rviz::Shape ( shape_start_point_, scene_manager_, frame_node_ ) ) );
            boost::shared_ptr<rviz::Shape> startShape = startPts_.back();
            startShape->setColor ( color_start_point_ );
            startShape->setPosition ( Ogre::Vector3 ( p0.position.x, p0.position.y, p0.position.z ) );
            startShape->setOrientation ( Ogre::Quaternion ( p0.orientation.x, p0.orientation.y, p0.orientation.z, p0.orientation.w ) );
            startShape->setScale ( Ogre::Vector3 ( scale_start_point_, scale_start_point_, scale_start_point_ ) );
        }

        if ( show_end_points_ ) {
            endPts_.push_back ( boost::shared_ptr<rviz::Shape> ( new rviz::Shape ( shape_end_point_, scene_manager_, frame_node_ ) ) );
            boost::shared_ptr<rviz::Shape> endShape = endPts_.back();
            endShape->setColor ( color_end_point_ );
            endShape->setPosition ( Ogre::Vector3 ( p1.position.x, p1.position.y, p1.position.z ) );
            endShape->setOrientation ( Ogre::Quaternion ( p1.orientation.x, p1.orientation.y, p1.orientation.z, p1.orientation.w ) );
            endShape->setScale ( Ogre::Vector3 ( scale_end_point_, scale_end_point_, scale_end_point_ ) );
        }
        if ( show_center_points_ && (segment.type == tuw::ros_msgs::RouteSegment::ARC ))  {
            centerPts_.push_back ( boost::shared_ptr<rviz::Shape> ( new rviz::Shape ( shape_center_point_, scene_manager_, frame_node_ ) ) );
            boost::shared_ptr<rviz::Shape> centerShape = centerPts_.back();
            centerShape->setColor ( color_center_point_ );
            centerShape->setPosition ( Ogre::Vector3 ( pc.position.x, pc.position.y, pc.position.z ) );
            centerShape->setOrientation ( Ogre::Quaternion ( pc.orientation.x, pc.orientation.y, pc.orientation.z, pc.orientation.w ) );
            centerShape->setScale ( Ogre::Vector3 ( scale_center_point_, scale_center_point_, scale_center_point_ ) );
        }
        if ( show_lines_ && ( segment.type == tuw::ros_msgs::RouteSegment::LINE ) ) {
            lines_.push_back ( boost::shared_ptr<rviz::Line> ( new rviz::Line ( scene_manager_, frame_node_ ) ) );
            boost::shared_ptr<rviz::Line> line = lines_.back();
            line->setColor ( color_lines_ );
            line->setPoints ( Ogre::Vector3 ( p0.position.x, p0.position.y, p0.position.z ), Ogre::Vector3 ( p1.position.x, p1.position.y, p1.position.z ) );
            line->setScale ( Ogre::Vector3 ( 1, 1, 1 ) );
        }

        if ( show_acrs_ && ( segment.type == tuw::ros_msgs::RouteSegment::ARC ) ) {
            double angle_resolution = M_PI/45.;
            std::vector<geometry_msgs::PosePtr> poses;
            
            segment.sample_equal_angle(poses, angle_resolution, 0);
            
            Ogre::Vector3 v0( p0.position.x, p0.position.y, p0.position.z);
            Ogre::Vector3 v1;
            for ( int i = 1; i < poses.size(); i++) {
                arcs_.push_back ( boost::shared_ptr<rviz::Line> ( new rviz::Line ( scene_manager_, frame_node_ ) ) );
                boost::shared_ptr<rviz::Line> line = arcs_.back();
                v1 = Ogre::Vector3 ( poses[i]->position.x, poses[i]->position.y, poses[i]->position.z);
                line->setColor ( color_arcs_ );
                line->setPoints ( v0, v1 );
                line->setScale ( Ogre::Vector3 ( 1, 1, 1 ) );
                v0 = v1;
            }
            arcs_.push_back ( boost::shared_ptr<rviz::Line> ( new rviz::Line ( scene_manager_, frame_node_ ) ) );
            boost::shared_ptr<rviz::Line> line = arcs_.back();
            v1 = Ogre::Vector3 ( p1.position.x, p1.position.y, p1.position.z);
            line->setColor ( color_arcs_ );
            line->setPoints ( v0, v1 );
            line->setScale ( Ogre::Vector3 ( 1, 1, 1 ) );
            
        }

    }

}
Пример #3
0
bool MeshTopologyTests::test1DMesh()
{
  bool success = true;

  CellTopoPtr line_2 = Camellia::CellTopology::line();
  RefinementPatternPtr lineRefPattern = RefinementPattern::regularRefinementPatternLine();

  vector<double> v0(1,0.0), v1(1,1.0), v2(1,3.0);
  vector< vector<double> > vertices;
  vertices.push_back(v0);
  vertices.push_back(v1);
  vertices.push_back(v2);

  vector< unsigned > elemVertexList(2);
  elemVertexList[0] = 0;
  elemVertexList[1] = 1;
  vector< vector< unsigned > > elementVertices;
  elementVertices.push_back(elemVertexList);

  elemVertexList[0] = 1;
  elemVertexList[1] = 2;
  elementVertices.push_back(elemVertexList);

  vector< CellTopoPtr > cellTopos(2,line_2);

  MeshGeometryPtr meshGeometry = Teuchos::rcp( new MeshGeometry(vertices, elementVertices, cellTopos) );

  MeshTopology mesh(meshGeometry);

  if (mesh.cellCount() != 2)
  {
    success = false;
    cout << "After initialization, mesh doesn't have the expected number of cells.\n";
  }

  if (mesh.activeCellCount() != 2)
  {
    success = false;
    cout << "After initialization, mesh doesn't have the expected number of active cells.\n";
  }

  mesh.refineCell(0, lineRefPattern, mesh.cellCount());
  if (mesh.cellCount() != 4)
  {
    success = false;
    cout << "After refinement, mesh doesn't have the expected number of cells.\n";
  }

  if (mesh.activeCellCount() != 3)
  {
    success = false;
    cout << "After refinement, mesh doesn't have the expected number of active cells.\n";
  }

  mesh.refineCell(2, lineRefPattern, mesh.cellCount());
  if (mesh.cellCount() != 6)
  {
    success = false;
    cout << "After 2nd refinement, mesh doesn't have the expected number of cells.\n";
  }

  if (mesh.activeCellCount() != 4)
  {
    success = false;
    cout << "After 2nd refinement, mesh doesn't have the expected number of active cells.\n";
  }

  return success;
}
Пример #4
0
int main()
{

  // User-defined main parameters
  double t0 = 0;                   // initial computation time
  double T = 20.0;                 // end of computation time
  double h = 0.005;                // time step
  double position_init = 10.0;     // initial position
  double velocity_init = 0.0;      // initial velocity

  double g = 9.81;
  double theta = 0.5;              // theta for MoreauJeanOSI integrator

  // -----------------------------------------
  // --- Dynamical systems && interactions ---
  // -----------------------------------------

  try
  {

    // ------------
    // --- Init ---
    // ------------

    std::cout << "====> Model loading ..." << std::endl << std::endl;


    // -- OneStepIntegrators --
    SP::OneStepIntegrator osi;
    osi.reset(new MoreauJeanOSI(theta));

    // -- Model --
    SP::Model model(new Model(t0, T));

    std::vector<SP::BulletWeightedShape> shapes;

    // note: no rebound with a simple Bullet Box, why ?
    // the distance after the broadphase contact detection is negative
    // and then stay negative.
    // SP::btCollisionShape box(new btBoxShape(btVector3(1,1,1)));
    // SP::BulletWeightedShape box1(new BulletWeightedShape(box,1.0));
    // This is ok if we build one with btConveHullShape
    SP::btCollisionShape box(new btConvexHullShape());
    {
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, -1.0, 1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(-1.0, 1.0, 1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, 1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, 1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, -1.0));
      std11::static_pointer_cast<btConvexHullShape>(box)->addPoint(btVector3(1.0, -1.0, 1.0));
    }
    SP::BulletWeightedShape box1(new BulletWeightedShape(box, 1.0));
    shapes.push_back(box1);

    SP::SiconosVector q0(new SiconosVector(7));
    SP::SiconosVector v0(new SiconosVector(6));
    v0->zero();
    q0->zero();

    (*q0)(2) = position_init;
    (*q0)(3) = 1.0;
    (*v0)(2) = velocity_init;

    // -- The dynamical system --
    // -- the default contactor is the shape given in the constructor
    // -- the contactor id is 0
    SP::BulletDS body(new BulletDS(box1, q0, v0));

    // -- Set external forces (weight) --
    SP::SiconosVector FExt;
    FExt.reset(new SiconosVector(3)); //
    FExt->zero();
    FExt->setValue(2, - g * box1->mass());
    body->setFExtPtr(FExt);

    // -- Add the dynamical system in the non smooth dynamical system
    osi->insertDynamicalSystem(body);
    model->nonSmoothDynamicalSystem()->insertDynamicalSystem(body);

    SP::btCollisionObject ground(new btCollisionObject());
    ground->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
    SP::btCollisionShape groundShape(new btBoxShape(btVector3(30, 30, .5)));
    btMatrix3x3 basis;
    basis.setIdentity();
    ground->getWorldTransform().setBasis(basis);
    ground->setCollisionShape(&*groundShape);
    ground->getWorldTransform().getOrigin().setZ(-.50);

    // ------------------
    // --- Simulation ---
    // ------------------

    // -- Time discretisation --
    SP::TimeDiscretisation timedisc(new TimeDiscretisation(t0, h));

    // -- OneStepNsProblem --
    SP::FrictionContact osnspb(new FrictionContact(3));

    // -- Some configuration

    osnspb->numericsSolverOptions()->iparam[0] = 1000; // Max number of
    // iterations
    osnspb->numericsSolverOptions()->dparam[0] = 1e-5; // Tolerance


    osnspb->setMaxSize(16384);                        // max number of
    // interactions

    osnspb->setMStorageType(1);                      // Sparse storage

    osnspb->setNumericsVerboseMode(0);               // 0 silent, 1
    // verbose

    osnspb->setKeepLambdaAndYState(true);            // inject
    // previous
    // solution

    // --- Simulation initialization ---

    std::cout << "====> Simulation initialisation ..." << std::endl << std::endl;

    int N = ceil((T - t0) / h); // Number of time steps


    SP::NonSmoothLaw nslaw(new NewtonImpactFrictionNSL(0.8, 0., 0.0, 3));

    // -- The space filter performs broadphase collision detection
    SP::BulletSpaceFilter space_filter(new BulletSpaceFilter(model));

    // -- insert a non smooth law for contactors id 0
    space_filter->insert(nslaw, 0, 0);

    // -- add multipoint iterations, this is needed to gather at least
    // -- 3 contact points and avoid objects penetration, see Bullet
    // -- documentation
    space_filter->collisionConfiguration()->setConvexConvexMultipointIterations();
    space_filter->collisionConfiguration()->setPlaneConvexMultipointIterations();

    // -- The ground is a static object
    // -- we give it a group contactor id : 0
    space_filter->addStaticObject(ground, 0);

    // -- MoreauJeanOSI Time Stepping with Bullet Dynamical Systems
    SP::BulletTimeStepping simulation(new BulletTimeStepping(timedisc));

    simulation->insertIntegrator(osi);
    simulation->insertNonSmoothProblem(osnspb);

    model->initialize(simulation);

    std::cout << "====> End of initialisation ..." << std::endl << std::endl;

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 4;
    SimpleMatrix dataPlot(N + 1, outputSize);
    dataPlot.zero();

    SP::SiconosVector q = body->q();
    SP::SiconosVector v = body->velocity();

    dataPlot(0, 0) = model->t0();
    dataPlot(0, 1) = (*q)(2);
    dataPlot(0, 2) = (*v)(2);

    // --- Time loop ---

    std::cout << "====> Start computation ... " << std::endl << std::endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 1;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();

    while (simulation->hasNextEvent())
    {
      space_filter->buildInteractions(model->currentTime());

      simulation->computeOneStep();

      // --- Get values to be plotted ---
      dataPlot(k, 0) =  simulation->nextTime();
      dataPlot(k, 1) = (*q)(2);
      dataPlot(k, 2) = (*v)(2);

      // If broadphase collision detection shows some contacts then we may
      // display contact forces.
      if (space_filter->collisionWorld()->getDispatcher()->getNumManifolds() > 0)
      {

        // we *must* have an indexSet0, filled by Bullet broadphase
        // collision detection and an indexSet1, filled by
        // TimeStepping::updateIndexSet with the help of Bullet
        // getDistance() function
        if (model->nonSmoothDynamicalSystem()->topology()->numberOfIndexSet() == 2)
        {
          SP::InteractionsGraph index1 = simulation->indexSet(1);

          // This is the narrow phase contact detection : if
          // TimeStepping::updateIndexSet has filled indexSet1 then we
          // have some contact forces to display
          if (index1->size() > 0)
          {

            // Four contact points for a cube with a side facing the
            // ground. Note : changing Bullet margin for collision
            // detection may lead this assertion to be false.
            if (index1->size() == 4)
            {
              InteractionsGraph::VIterator iur = index1->begin();

              // different version of bullet may not gives the same
              // contact points! So we only keep the summation.
              dataPlot(k, 3) =
                index1->bundle(*iur)-> lambda(1)->norm2() +
                index1->bundle(*++iur)->lambda(1)->norm2() +
                index1->bundle(*++iur)->lambda(1)->norm2() +
                index1->bundle(*++iur)->lambda(1)->norm2();
            }
          }
        }
      }

      simulation->nextStep();
      ++show_progress;
      k++;
    }


    std::cout << std::endl << "End of computation - Number of iterations done: " << k - 1 << std::endl;
    std::cout << "Computation Time " << time.elapsed()  << std::endl;

    // --- Output files ---
    std::cout << "====> Output file writing ..." << std::endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");

    // Comparison with a reference file
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
    ioMatrix::read("result.ref", "ascii", dataPlotRef);

    if ((dataPlot - dataPlotRef).normInf() > 1e-12)
    {
      std::cout << "Warning. The result is rather different from the reference file : "
                << (dataPlot - dataPlotRef).normInf() << std::endl;
      return 1;
    }



  }

  catch (SiconosException e)
  {
    std::cout << e.report() << std::endl;
    exit(1);
  }
  catch (...)
  {
    std::cout << "Exception caught in BulletBouncingBox" << std::endl;
    exit(1);
  }

  return 0;
}
void simple_read_write_tests() {
    tbb::flow::graph g;
    tbb::flow::write_once_node<R> n(g);

    for ( int t = 0; t < T; ++t ) {
        R v0(0);
        harness_counting_receiver<R> r[M];

        ASSERT( n.is_valid() == false, NULL );
        ASSERT( n.try_get( v0 ) == false, NULL );

        if ( t % 2 ) {
            ASSERT( n.try_put( static_cast<R>(N+1) ), NULL );
            ASSERT( n.is_valid() == true, NULL );
            ASSERT( n.try_get( v0 ) == true, NULL );
            ASSERT( v0 == R(N+1), NULL );
       }

        for (int i = 0; i < M; ++i) {
           tbb::flow::make_edge( n, r[i] );
        }
      
        if ( t%2 ) {
            for (int i = 0; i < M; ++i) {
                 size_t c = r[i].my_count;
                 ASSERT( int(c) == 1, NULL );
            }
        }

        for (int i = 1; i <= N; ++i ) {
            R v1(static_cast<R>(i));

            bool result = n.try_put( v1 );
            if ( !(t%2) && i == 1 ) 
                ASSERT( result == true, NULL );
            else
                ASSERT( result == false, NULL );

            ASSERT( n.is_valid() == true, NULL );

            for (int j = 0; j < N; ++j ) {
                R v2(0);
                ASSERT( n.try_get( v2 ), NULL );
                if ( t%2 ) 
                    ASSERT( R(N+1) == v2, NULL );
                else 
                    ASSERT( R(1) == v2, NULL );
            }
        }
        for (int i = 0; i < M; ++i) {
             size_t c = r[i].my_count;
             ASSERT( int(c) == 1, NULL );
        }
        for (int i = 0; i < M; ++i) {
           tbb::flow::remove_edge( n, r[i] );
        }
        ASSERT( n.try_put( R(0) ) == false, NULL );
        for (int i = 0; i < M; ++i) {
             size_t c = r[i].my_count;
             ASSERT( int(c) == 1, NULL );
        }
        n.clear();
        ASSERT( n.is_valid() == false, NULL );
        ASSERT( n.try_get( v0 ) == false, NULL );
    }
}