<commit_before>/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

#include "mitkCorrectorTool2D.h"
#include "mitkCorrectorAlgorithm.h"

#include "mitkAbstractTransformGeometry.h"
#include "mitkBaseRenderer.h"
#include "mitkImageReadAccessor.h"
#include "mitkLabelSetImage.h"
#include "mitkRenderingManager.h"
#include "mitkToolManager.h"

#include "mitkCorrectorTool2D.xpm"
#include "mitkLabelSetImage.h"

// us
#include <usGetModuleContext.h>
#include <usModule.h>
#include <usModuleContext.h>
#include <usModuleResource.h>

namespace mitk
{
  MITK_TOOL_MACRO(MITKSEGMENTATION_EXPORT, CorrectorTool2D, "Correction tool");
}

mitk::CorrectorTool2D::CorrectorTool2D(int paintingPixelValue)
  : FeedbackContourTool("PressMoveRelease"), m_PaintingPixelValue(paintingPixelValue)
{
  GetFeedbackContour()->SetClosed(false); // don't close the contour to a polygon
}

mitk::CorrectorTool2D::~CorrectorTool2D()
{
}

void mitk::CorrectorTool2D::ConnectActionsAndFunctions()
{
  CONNECT_FUNCTION("PrimaryButtonPressed", OnMousePressed);
  CONNECT_FUNCTION("Move", OnMouseMoved);
  CONNECT_FUNCTION("Release", OnMouseReleased);
}

const char **mitk::CorrectorTool2D::GetXPM() const
{
  return mitkCorrectorTool2D_xpm;
}

us::ModuleResource mitk::CorrectorTool2D::GetIconResource() const
{
  us::Module *module = us::GetModuleContext()->GetModule();
  us::ModuleResource resource = module->GetResource("Correction_48x48.png");
  return resource;
}

us::ModuleResource mitk::CorrectorTool2D::GetCursorIconResource() const
{
  us::Module *module = us::GetModuleContext()->GetModule();
  us::ModuleResource resource = module->GetResource("Correction_Cursor_32x32.png");
  return resource;
}

const char *mitk::CorrectorTool2D::GetName() const
{
  return "Correction";
}

void mitk::CorrectorTool2D::Activated()
{
  Superclass::Activated();
}

void mitk::CorrectorTool2D::Deactivated()
{
  Superclass::Deactivated();
}

void mitk::CorrectorTool2D::OnMousePressed(StateMachineAction *, InteractionEvent *interactionEvent)
{
  auto *positionEvent = dynamic_cast<mitk::InteractionPositionEvent *>(interactionEvent);
  if (!positionEvent)
    return;

  m_LastEventSender = positionEvent->GetSender();
  m_LastEventSlice = m_LastEventSender->GetSlice();

  int timestep = positionEvent->GetSender()->GetTimeStep();
  ContourModel *contour = FeedbackContourTool::GetFeedbackContour();
  contour->Initialize();
  contour->Expand(timestep + 1);
  contour->SetClosed(false, timestep);
  mitk::Point3D point = positionEvent->GetPositionInWorld();
  contour->AddVertex(point, timestep);

  FeedbackContourTool::SetFeedbackContourVisible(true);
}

void mitk::CorrectorTool2D::OnMouseMoved(StateMachineAction *, InteractionEvent *interactionEvent)
{
  auto *positionEvent = dynamic_cast<mitk::InteractionPositionEvent *>(interactionEvent);
  if (!positionEvent)
    return;

  int timestep = positionEvent->GetSender()->GetTimeStep();
  ContourModel *contour = FeedbackContourTool::GetFeedbackContour();
  mitk::Point3D point = positionEvent->GetPositionInWorld();
  contour->AddVertex(point, timestep);

  assert(positionEvent->GetSender()->GetRenderWindow());
  mitk::RenderingManager::GetInstance()->RequestUpdate(positionEvent->GetSender()->GetRenderWindow());
}

void mitk::CorrectorTool2D::OnMouseReleased(StateMachineAction *, InteractionEvent *interactionEvent)
{
  // 1. Hide the feedback contour, find out which slice the user clicked, find out which slice of the toolmanager's
  // working image corresponds to that
  FeedbackContourTool::SetFeedbackContourVisible(false);

  auto *positionEvent = dynamic_cast<mitk::InteractionPositionEvent *>(interactionEvent);
  // const PositionEvent* positionEvent = dynamic_cast<const PositionEvent*>(stateEvent->GetEvent());
  if (!positionEvent)
    return;

  assert(positionEvent->GetSender()->GetRenderWindow());
  mitk::RenderingManager::GetInstance()->RequestUpdate(positionEvent->GetSender()->GetRenderWindow());

  DataNode *workingNode(m_ToolManager->GetWorkingData(0));
  if (!workingNode)
    return;

  auto *image = dynamic_cast<Image *>(workingNode->GetData());
  const PlaneGeometry *planeGeometry((positionEvent->GetSender()->GetCurrentWorldPlaneGeometry()));
  if (!image || !planeGeometry)
    return;

  const auto *abstractTransformGeometry(
    dynamic_cast<const AbstractTransformGeometry *>(positionEvent->GetSender()->GetCurrentWorldPlaneGeometry()));
  if (!image || abstractTransformGeometry)
    return;

  // 2. Slice is known, now we try to get it as a 2D image and project the contour into index coordinates of this slice
  m_WorkingSlice = FeedbackContourTool::GetAffectedImageSliceAs2DImage(positionEvent, image);

  if (m_WorkingSlice.IsNull())
  {
    MITK_ERROR << "Unable to extract slice." << std::endl;
    return;
  }

  int timestep = positionEvent->GetSender()->GetTimeStep();
  mitk::ContourModel::Pointer singleTimestepContour = mitk::ContourModel::New();

  auto it = FeedbackContourTool::GetFeedbackContour()->Begin(timestep);
  auto end = FeedbackContourTool::GetFeedbackContour()->End(timestep);

  while (it != end)
  {
    singleTimestepContour->AddVertex((*it)->Coordinates);
    it++;
  }

  CorrectorAlgorithm::Pointer algorithm = CorrectorAlgorithm::New();
  algorithm->SetInput(m_WorkingSlice);
  algorithm->SetContour(singleTimestepContour);

  mitk::LabelSetImage::Pointer labelSetImage = dynamic_cast<LabelSetImage *>(workingNode->GetData());
  int workingColorId(1);
  if (labelSetImage.IsNotNull())
  {
    workingColorId = labelSetImage->GetActiveLabel()->GetValue();
    algorithm->SetFillColor(workingColorId);
  }
  try
  {
    algorithm->UpdateLargestPossibleRegion();
  }
  catch (std::exception &e)
  {
    MITK_ERROR << "Caught exception '" << e.what() << "'" << std::endl;
  }

  mitk::Image::Pointer resultSlice = mitk::Image::New();
  resultSlice->Initialize(algorithm->GetOutput());

  if (labelSetImage.IsNotNull())
  {
    mitk::Image::Pointer erg1 = FeedbackContourTool::GetAffectedImageSliceAs2DImage(positionEvent, image);
    SegTool2D::WritePreviewOnWorkingImage(erg1, algorithm->GetOutput(), image, workingColorId, 0);
    SegTool2D::WriteBackSegmentationResult(positionEvent, erg1);
  }
  else
  {
    mitk::ImageReadAccessor imAccess(algorithm->GetOutput());
    resultSlice->SetVolume(imAccess.GetData());
    this->WriteBackSegmentationResult(positionEvent, resultSlice);
  }
}
<commit_msg>Fix T28076 by using the new function for retrieving the active pixel value<commit_after>/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

#include "mitkCorrectorTool2D.h"
#include "mitkCorrectorAlgorithm.h"

#include "mitkAbstractTransformGeometry.h"
#include "mitkBaseRenderer.h"
#include "mitkImageReadAccessor.h"
#include "mitkLabelSetImage.h"
#include "mitkRenderingManager.h"
#include "mitkToolManager.h"

#include "mitkCorrectorTool2D.xpm"
#include "mitkLabelSetImage.h"

// us
#include <usGetModuleContext.h>
#include <usModule.h>
#include <usModuleContext.h>
#include <usModuleResource.h>

namespace mitk
{
  MITK_TOOL_MACRO(MITKSEGMENTATION_EXPORT, CorrectorTool2D, "Correction tool");
}

mitk::CorrectorTool2D::CorrectorTool2D(int paintingPixelValue)
  : FeedbackContourTool("PressMoveRelease"), m_PaintingPixelValue(paintingPixelValue)
{
  GetFeedbackContour()->SetClosed(false); // don't close the contour to a polygon
}

mitk::CorrectorTool2D::~CorrectorTool2D()
{
}

void mitk::CorrectorTool2D::ConnectActionsAndFunctions()
{
  CONNECT_FUNCTION("PrimaryButtonPressed", OnMousePressed);
  CONNECT_FUNCTION("Move", OnMouseMoved);
  CONNECT_FUNCTION("Release", OnMouseReleased);
}

const char **mitk::CorrectorTool2D::GetXPM() const
{
  return mitkCorrectorTool2D_xpm;
}

us::ModuleResource mitk::CorrectorTool2D::GetIconResource() const
{
  us::Module *module = us::GetModuleContext()->GetModule();
  us::ModuleResource resource = module->GetResource("Correction_48x48.png");
  return resource;
}

us::ModuleResource mitk::CorrectorTool2D::GetCursorIconResource() const
{
  us::Module *module = us::GetModuleContext()->GetModule();
  us::ModuleResource resource = module->GetResource("Correction_Cursor_32x32.png");
  return resource;
}

const char *mitk::CorrectorTool2D::GetName() const
{
  return "Correction";
}

void mitk::CorrectorTool2D::Activated()
{
  Superclass::Activated();
}

void mitk::CorrectorTool2D::Deactivated()
{
  Superclass::Deactivated();
}

void mitk::CorrectorTool2D::OnMousePressed(StateMachineAction *, InteractionEvent *interactionEvent)
{
  auto *positionEvent = dynamic_cast<mitk::InteractionPositionEvent *>(interactionEvent);
  if (!positionEvent)
    return;

  m_LastEventSender = positionEvent->GetSender();
  m_LastEventSlice = m_LastEventSender->GetSlice();

  int timestep = positionEvent->GetSender()->GetTimeStep();
  ContourModel *contour = FeedbackContourTool::GetFeedbackContour();
  contour->Initialize();
  contour->Expand(timestep + 1);
  contour->SetClosed(false, timestep);
  mitk::Point3D point = positionEvent->GetPositionInWorld();
  contour->AddVertex(point, timestep);

  FeedbackContourTool::SetFeedbackContourVisible(true);
}

void mitk::CorrectorTool2D::OnMouseMoved(StateMachineAction *, InteractionEvent *interactionEvent)
{
  auto *positionEvent = dynamic_cast<mitk::InteractionPositionEvent *>(interactionEvent);
  if (!positionEvent)
    return;

  int timestep = positionEvent->GetSender()->GetTimeStep();
  ContourModel *contour = FeedbackContourTool::GetFeedbackContour();
  mitk::Point3D point = positionEvent->GetPositionInWorld();
  contour->AddVertex(point, timestep);

  assert(positionEvent->GetSender()->GetRenderWindow());
  mitk::RenderingManager::GetInstance()->RequestUpdate(positionEvent->GetSender()->GetRenderWindow());
}

void mitk::CorrectorTool2D::OnMouseReleased(StateMachineAction *, InteractionEvent *interactionEvent)
{
  // 1. Hide the feedback contour, find out which slice the user clicked, find out which slice of the toolmanager's
  // working image corresponds to that
  FeedbackContourTool::SetFeedbackContourVisible(false);

  auto *positionEvent = dynamic_cast<mitk::InteractionPositionEvent *>(interactionEvent);
  // const PositionEvent* positionEvent = dynamic_cast<const PositionEvent*>(stateEvent->GetEvent());
  if (!positionEvent)
    return;

  assert(positionEvent->GetSender()->GetRenderWindow());
  mitk::RenderingManager::GetInstance()->RequestUpdate(positionEvent->GetSender()->GetRenderWindow());

  DataNode *workingNode(m_ToolManager->GetWorkingData(0));
  if (!workingNode)
    return;

  auto *workingImage = dynamic_cast<Image *>(workingNode->GetData());
  const PlaneGeometry *planeGeometry((positionEvent->GetSender()->GetCurrentWorldPlaneGeometry()));
  if (!workingImage || !planeGeometry)
    return;

  const auto *abstractTransformGeometry(
    dynamic_cast<const AbstractTransformGeometry *>(positionEvent->GetSender()->GetCurrentWorldPlaneGeometry()));
  if (!workingImage || abstractTransformGeometry)
    return;

  // 2. Slice is known, now we try to get it as a 2D image and project the contour into index coordinates of this slice
  m_WorkingSlice = FeedbackContourTool::GetAffectedImageSliceAs2DImage(positionEvent, workingImage);

  if (m_WorkingSlice.IsNull())
  {
    MITK_ERROR << "Unable to extract slice." << std::endl;
    return;
  }

  int timestep = positionEvent->GetSender()->GetTimeStep();
  mitk::ContourModel::Pointer singleTimestepContour = mitk::ContourModel::New();

  auto it = FeedbackContourTool::GetFeedbackContour()->Begin(timestep);
  auto end = FeedbackContourTool::GetFeedbackContour()->End(timestep);

  while (it != end)
  {
    singleTimestepContour->AddVertex((*it)->Coordinates);
    it++;
  }

  CorrectorAlgorithm::Pointer algorithm = CorrectorAlgorithm::New();
  algorithm->SetInput(m_WorkingSlice);
  algorithm->SetContour(singleTimestepContour);

  int activePixelValue = ContourModelUtils::GetActivePixelValue(workingImage);
  algorithm->SetFillColor(activePixelValue);

  try
  {
    algorithm->UpdateLargestPossibleRegion();
  }
  catch (std::exception &e)
  {
    MITK_ERROR << "Caught exception '" << e.what() << "'" << std::endl;
  }

  mitk::Image::Pointer resultSlice = mitk::Image::New();
  resultSlice->Initialize(algorithm->GetOutput());

  auto* labelSetImage = dynamic_cast<LabelSetImage*>(workingImage);
  if (nullptr != labelSetImage)
  {
    mitk::Image::Pointer erg1 = FeedbackContourTool::GetAffectedImageSliceAs2DImage(positionEvent, workingImage);
    SegTool2D::WritePreviewOnWorkingImage(erg1, algorithm->GetOutput(), workingImage, activePixelValue, 0);
    SegTool2D::WriteBackSegmentationResult(positionEvent, erg1);
  }
  else
  {
    mitk::ImageReadAccessor imAccess(algorithm->GetOutput());
    resultSlice->SetVolume(imAccess.GetData());
    this->WriteBackSegmentationResult(positionEvent, resultSlice);
  }
}
<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- *
 *                        OpenSim:  toyLeg_example.cpp                        *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): Matt S. DeMers                                                  *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

/*
 *  Below is an example of an OpenSim application that provides its own
 *  main() routine.  This application acts as an example for utilizing the
 *  ControllabeSpring actuator.
 */

// Author:  Matt DeMers

//==============================================================================
//==============================================================================
#include "PistonActuator.h"
#include "ControllableSpring.h"
#include <OpenSim/OpenSim.h>
#include "OpenSim/Common/STOFileAdapter.h"

using namespace OpenSim;
using namespace SimTK;

//______________________________________________________________________________
/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact
 */
int main()
{

    try {
        // Create a new OpenSim model
        Model osimModel;
        osimModel.setName("osimModel");
        osimModel.setAuthors("Matt DeMers");

        double Pi = SimTK::Pi;

        // Get the ground body
        Ground& ground = osimModel.updGround();
        ground.attachMeshGeometry("checkered_floor.vtp");

        // create linkage body
        double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;

        Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
        Vec3 linkageMassCenter(0,linkageLength/2,0);
        Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);

        OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);

        // Graphical representation
        Cylinder cyl;
        cyl.set_scale_factors(linkageDimensions);
        Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl1Frame->setName("Cyl1_frame");
        osimModel.addFrame(cyl1Frame);
        cyl.setFrameName("Cyl1_frame");
        linkage1->addGeometry(cyl);

        linkage1->attachGeometry(Sphere(0.1));

        // Create a second linkage body
        OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
        linkage2->setName("linkage2");
        Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl2Frame->setName("Cyl2_frame");
        osimModel.addFrame(cyl2Frame);
        (linkage2->upd_geometry(0)).setFrameName("Cyl2_frame");
        // Create a block to be the pelvis
        double blockMass = 20.0, blockSideLength = 0.2;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
        block->attachGeometry(Brick(SimTK::Vec3(0.05, 0.05, 0.05)));

        // Create 1 degree-of-freedom pin joints between the bodies to create a kinematic chain from ground through the block
        Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);

        PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1,
            locationInChild, orientationInChild);

        PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
            locationInChild, orientationInChild);

        PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
            locationInChild, orientationInChild);

        double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
        CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
        ankleCoordinateSet[0].setName("q1");
        ankleCoordinateSet[0].setRange(range);

        CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
        kneeCoordinateSet[0].setName("q2");
        kneeCoordinateSet[0].setRange(range);

        CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
        hipCoordinateSet[0].setName("q3");
        hipCoordinateSet[0].setRange(range);

        // Add the bodies to the model
        osimModel.addBody(linkage1);
        osimModel.addBody(linkage2);
        osimModel.addBody(block);

        // Add the joints to the model
        osimModel.addJoint(ankle);
        osimModel.addJoint(knee);
        osimModel.addJoint(hip);
        // Define constraints on the model
        //  Add a point on line constraint to limit the block to vertical motion

        Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
        PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
        osimModel.addConstraint(lineConstraint);

        // Add PistonActuator between the first linkage and the block
        Vec3 pointOnBodies(0);
        PistonActuator *piston = new PistonActuator();
        piston->setName("piston");
        piston->setBodyA(linkage1);
        piston->setBodyB(block);
        piston->setPointA(pointOnBodies);
        piston->setPointB(pointOnBodies);
        piston->setOptimalForce(200.0);
        piston->setPointsAreGlobal(false);

        osimModel.addForce(piston);
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Added ControllableSpring between the first linkage and the second block
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        ControllableSpring *spring = new ControllableSpring;
        spring->setName("spring");
        spring->setBodyA(block);
        spring->setBodyB(linkage1);
        spring->setPointA(pointOnBodies);
        spring->setPointB(pointOnBodies);
        spring->setOptimalForce(2000.0);
        spring->setPointsAreGlobal(false);
        spring->setRestLength(0.8);

        osimModel.addForce(spring);

        // define the simulation times
        double t0(0.0), tf(15);

        // create a controller to control the piston and spring actuators
        // the prescribed controller sets the controls as functions of time
        PrescribedController *legController = new PrescribedController();
        // give the legController control over all (two) model actuators
        legController->setActuators(osimModel.updActuators());

        // specify some control nodes for spring stiffness control
        double t[] = {0.0, 4.0, 7.0,  10.0, 15.0};
        double x[] = {1.0, 1.0, 0.25,  0.25, 5.0};

        // specify the control function for each actuator
        legController->prescribeControlForActuator("piston", new Constant(0.1));
        legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x));

        // add the controller to the model
        osimModel.addController(legController);

        // define the acceleration due to gravity
        osimModel.setGravity(Vec3(0, -9.80665, 0));

        // enable the model visualizer see the model in action, which can be
        // useful for debugging
        osimModel.setUseVisualizer(false);

        // Initialize system
        SimTK::State& si = osimModel.initSystem();

        // Pin joint initial states
        double q1_i = -Pi/4;
        double q2_i = - 2*q1_i;
        CoordinateSet &coordinates = osimModel.updCoordinateSet();
        coordinates[0].setValue(si, q1_i, true);
        coordinates[1].setValue(si,q2_i, true);

        // Setup integrator and manager
        SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-3);

        ForceReporter *forces = new ForceReporter(&osimModel);
        osimModel.updAnalysisSet().adoptAndAppend(forces);
        Manager manager(osimModel, integrator);

        //Examine the model
        osimModel.printDetailedInfo(si, std::cout);
        // Save the model
        osimModel.print("toyLeg.osim");
        // Print out the initial position and velocity states
        si.getQ().dump("Initial q's");
        si.getU().dump("Initial u's");
        std::cout << "Initial time: " << si.getTime() << std::endl;

        // Integrate
        manager.setInitialTime(t0);
        manager.setFinalTime(tf);
        std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl;
        manager.integrate(si);

        // Save results
        auto controlsTable = osimModel.getControlsTable();
        STOFileAdapter::write(controlsTable, "SpringActuatedLeg_controls.sto");

        auto statesTable = manager.getStatesTable();
        osimModel.updSimbodyEngine().convertRadiansToDegrees(statesTable);
        STOFileAdapter::write(statesTable,
                              "SpringActuatedLeg_states_degrees.sto");

        auto forcesTable = forces->getForcesTable();
        STOFileAdapter::write(forcesTable, "actuator_forces.sto");
    }
    catch (const std::exception& ex)
    {
        std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
        return 1;
    }

    std::cout << "Done." << std::endl;
    return 0;
}
<commit_msg>update attachGeometry calls in toyLeg_example.cpp<commit_after>/* -------------------------------------------------------------------------- *
 *                        OpenSim:  toyLeg_example.cpp                        *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): Matt S. DeMers                                                  *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

/*
 *  Below is an example of an OpenSim application that provides its own
 *  main() routine.  This application acts as an example for utilizing the
 *  ControllabeSpring actuator.
 */

// Author:  Matt DeMers

//==============================================================================
//==============================================================================
#include "PistonActuator.h"
#include "ControllableSpring.h"
#include <OpenSim/OpenSim.h>
#include "OpenSim/Common/STOFileAdapter.h"

using namespace OpenSim;
using namespace SimTK;

//______________________________________________________________________________
/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact
 */
int main()
{

    try {
        // Create a new OpenSim model
        Model osimModel;
        osimModel.setName("osimModel");
        osimModel.setAuthors("Matt DeMers");

        double Pi = SimTK::Pi;

        // Get the ground body
        Ground& ground = osimModel.updGround();
        ground.attachGeometry(new Mesh("checkered_floor.vtp"));

        // create linkage body
        double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;

        Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
        Vec3 linkageMassCenter(0,linkageLength/2,0);
        Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);

        OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);
        linkage1->attachGeometry(new Sphere(0.1));

        // Graphical representation
        Cylinder cyl(linkageDiameter/2, linkageLength);
        Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1,
            Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl1Frame->setName("Cyl1_frame");
        cyl1Frame->attachGeometry(cyl.clone());
        osimModel.addFrame(cyl1Frame);

        // Create a second linkage body as a clone of the first
        OpenSim::Body* linkage2 = linkage1->clone();
        linkage2->setName("linkage2");
        Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2,
            Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
        cyl2Frame->setName("Cyl2_frame");
        osimModel.addFrame(cyl2Frame);
        (linkage2->upd_attached_geometry(0)).setFrameName("Cyl2_frame");

        // Create a block to be the pelvis
        double blockMass = 20.0, blockSideLength = 0.2;
        Vec3 blockMassCenter(0);
        Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
        OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
        block->attachGeometry(new Brick(SimTK::Vec3(0.05, 0.05, 0.05)));

        // Create 1 degree-of-freedom pin joints between the bodies to create a kinematic chain from ground through the block
        Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);

        PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1,
            locationInChild, orientationInChild);

        PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
            locationInChild, orientationInChild);

        PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
            locationInChild, orientationInChild);

        double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
        CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
        ankleCoordinateSet[0].setName("q1");
        ankleCoordinateSet[0].setRange(range);

        CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
        kneeCoordinateSet[0].setName("q2");
        kneeCoordinateSet[0].setRange(range);

        CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
        hipCoordinateSet[0].setName("q3");
        hipCoordinateSet[0].setRange(range);

        // Add the bodies to the model
        osimModel.addBody(linkage1);
        osimModel.addBody(linkage2);
        osimModel.addBody(block);

        // Add the joints to the model
        osimModel.addJoint(ankle);
        osimModel.addJoint(knee);
        osimModel.addJoint(hip);
        // Define constraints on the model
        //  Add a point on line constraint to limit the block to vertical motion

        Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
        PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
        osimModel.addConstraint(lineConstraint);

        // Add PistonActuator between the first linkage and the block
        Vec3 pointOnBodies(0);
        PistonActuator *piston = new PistonActuator();
        piston->setName("piston");
        piston->setBodyA(linkage1);
        piston->setBodyB(block);
        piston->setPointA(pointOnBodies);
        piston->setPointB(pointOnBodies);
        piston->setOptimalForce(200.0);
        piston->setPointsAreGlobal(false);

        osimModel.addForce(piston);
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        // Added ControllableSpring between the first linkage and the second block
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        ControllableSpring *spring = new ControllableSpring;
        spring->setName("spring");
        spring->setBodyA(block);
        spring->setBodyB(linkage1);
        spring->setPointA(pointOnBodies);
        spring->setPointB(pointOnBodies);
        spring->setOptimalForce(2000.0);
        spring->setPointsAreGlobal(false);
        spring->setRestLength(0.8);

        osimModel.addForce(spring);

        // define the simulation times
        double t0(0.0), tf(15);

        // create a controller to control the piston and spring actuators
        // the prescribed controller sets the controls as functions of time
        PrescribedController *legController = new PrescribedController();
        // give the legController control over all (two) model actuators
        legController->setActuators(osimModel.updActuators());

        // specify some control nodes for spring stiffness control
        double t[] = {0.0, 4.0, 7.0,  10.0, 15.0};
        double x[] = {1.0, 1.0, 0.25,  0.25, 5.0};

        // specify the control function for each actuator
        legController->prescribeControlForActuator("piston", new Constant(0.1));
        legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x));

        // add the controller to the model
        osimModel.addController(legController);

        // define the acceleration due to gravity
        osimModel.setGravity(Vec3(0, -9.80665, 0));

        // enable the model visualizer see the model in action, which can be
        // useful for debugging
        osimModel.setUseVisualizer(false);

        // Initialize system
        SimTK::State& si = osimModel.initSystem();

        // Pin joint initial states
        double q1_i = -Pi/4;
        double q2_i = - 2*q1_i;
        CoordinateSet &coordinates = osimModel.updCoordinateSet();
        coordinates[0].setValue(si, q1_i, true);
        coordinates[1].setValue(si,q2_i, true);

        // Setup integrator and manager
        SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
        integrator.setAccuracy(1.0e-3);

        ForceReporter *forces = new ForceReporter(&osimModel);
        osimModel.updAnalysisSet().adoptAndAppend(forces);
        Manager manager(osimModel, integrator);

        //Examine the model
        osimModel.printDetailedInfo(si, std::cout);
        // Save the model
        osimModel.print("toyLeg.osim");
        // Print out the initial position and velocity states
        si.getQ().dump("Initial q's");
        si.getU().dump("Initial u's");
        std::cout << "Initial time: " << si.getTime() << std::endl;

        // Integrate
        manager.setInitialTime(t0);
        manager.setFinalTime(tf);
        std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl;
        manager.integrate(si);

        // Save results
        auto controlsTable = osimModel.getControlsTable();
        STOFileAdapter::write(controlsTable, "SpringActuatedLeg_controls.sto");

        auto statesTable = manager.getStatesTable();
        osimModel.updSimbodyEngine().convertRadiansToDegrees(statesTable);
        STOFileAdapter::write(statesTable,
                              "SpringActuatedLeg_states_degrees.sto");

        auto forcesTable = forces->getForcesTable();
        STOFileAdapter::write(forcesTable, "actuator_forces.sto");
    }
    catch (const std::exception& ex)
    {
        std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
        return 1;
    }

    std::cout << "Done." << std::endl;
    return 0;
}
<|endoftext|>


<commit_before>/*
T0 DA for online calibration

Contact: Michal.Oledzki@cern.ch
Link: http://users.jyu.fi/~mioledzk/
Run Type: PHYSICS
DA Type: MON
Number of events needed: 500000
Input Files: inPhys.dat, external parameters
Output Files: daPhys.root, to be exported to the DAQ FXS
Trigger types used: PHYSICS_EVENT
------------------------------Alla
Now trigger type changed to CALICRATION_EVENT
to have data during test.
SOULD BE CHANGED BACK BEFORE BEAM
------------------------------- Alla
*/

#define FILE_OUT "daPhys.root"
#define FILE_IN "inPhys.dat"
#include <daqDA.h>
#include <event.h>
#include <monitor.h>

#include <Riostream.h>
#include <stdio.h>
#include <stdlib.h>

//AliRoot
#include <AliRawReaderDate.h>
#include <AliRawReader.h>
#include <AliT0RawReader.h>

//ROOT
#include "TROOT.h"
#include "TPluginManager.h"
#include "TFile.h"
#include "TKey.h"
#include "TObject.h"
#include "TBenchmark.h"
#include "TString.h"
#include "TH1.h"

int cbx, ccbx, t0bx, npmtA, npmtC;
float clx,cmx,cclx,ccmx, t0lx, t0hx;

/* Main routine
      Arguments:
      1- monitoring data source
*/
int main(int argc, char **argv) {
//int main(){
  int status;

  /* magic line */
  gROOT->GetPluginManager()->AddHandler("TVirtualStreamerInfo",
                                        "*",
                                        "TStreamerInfo",
                                        "RIO",
                                        "TStreamerInfo()");

  if(daqDA_DB_getFile(FILE_IN, FILE_IN)){
    printf("Couldn't get input file >>inPhys.dat<< from DAQ_DB !!!\n");
    return -1;
  }

  FILE *inp;
  char c;
  inp = fopen(FILE_IN, "r");
  if(!inp){
    printf("Input file >>inPhys.dat<< not found !!!\n");
    return -1;
  }

  while((c=getc(inp))!=EOF) {
    switch(c) {
    case 'a': {fscanf(inp, "%d", &ccbx ); break;} //N of X bins hCFD1_CFD
    case 'b': {fscanf(inp, "%f", &cclx ); break;} //Low x hCFD1_CFD
    case 'c': {fscanf(inp, "%f", &ccmx ); break;} //High x hCFD1_CF
    case 'd': {fscanf(inp, "%d", &npmtC ); break;} //number of reference PMTC
    case 'e': {fscanf(inp, "%d", &npmtA ); break;} //number of reference PMTA
    case 'f': {fscanf(inp, "%d", &t0bx ); break;} //N of X bins hT0
    case 'g': {fscanf(inp, "%f", &t0lx ); break;} //Low x hT0
    case 'k': {fscanf(inp, "%f", &t0hx ); break;} //High x hT0
    }
  }
  fclose(inp);

  if (argc!=2) {
    printf("Wrong number of arguments\n");
    return -1;
  }

  /* define data source : this is argument 1 */
  status=monitorSetDataSource( argv[1] );
  if (status!=0) {
    printf("monitorSetDataSource() failed : %s\n",monitorDecodeError(status));
    return -1;
  }

  /* declare monitoring program */
  status=monitorDeclareMp( __FILE__ );
  if (status!=0) {
    printf("monitorDeclareMp() failed : %s\n",monitorDecodeError(status));
    return -1;
  }

  /* define wait event timeout - 1s max */
  monitorSetNowait();
  monitorSetNoWaitNetworkTimeout(1000);

  /* log start of process */
  printf("T0 monitoring program started\n");

  // Allocation of histograms - start

  TH1F *hCFD1minCFD[24];

  for(Int_t ic=0; ic<24; ic++) {
    hCFD1minCFD[ic] = new TH1F(Form("CFD1minCFD%d",ic+1),"CFD-CFD",ccbx,cclx,ccmx);
  }
  TH1F *hVertex = new TH1F("hVertex","T0 time",t0bx,t0lx,t0hx);

  // Allocation of histograms - end

  Int_t iev=0;
  /* main loop (infinite) */
  for(;;) {
    struct eventHeaderStruct *event;
    eventTypeType eventT;

    /* check shutdown condition */
    if (daqDA_checkShutdown()) {break;}

    /* get next event (blocking call until timeout) */
    status=monitorGetEventDynamic((void **)&event);
    if (status==(int)MON_ERR_EOF) {
      printf ("End of File detected\n");
      break; /* end of monitoring file has been reached */
    }

    if (status!=0) {
      printf("monitorGetEventDynamic() failed : %s\n",monitorDecodeError(status));
      break;
    }

    /* retry if got no event */
    if (event==NULL) {
      continue;
    }

    /* use event - here, just write event id to result file */
    eventT=event->eventType;

    switch (event->eventType){

      case START_OF_RUN:
	break;

    case END_OF_RUN:
      break;

        case PHYSICS_EVENT:
	  //    case CALIBRATION_EVENT:
      iev++;

      if(iev==1){
	printf("First event - %i\n",iev);
      }

      // Initalize raw-data reading and decoding
      AliRawReader *reader = new AliRawReaderDate((void*)event);

      // Enable the following two lines in case of real-data
      reader->RequireHeader(kTRUE);
      AliT0RawReader *start = new AliT0RawReader(reader, kTRUE);

      // Read raw data
      Int_t allData[105][5];
      for(Int_t i0=0;i0<105;i0++)
      	for(Int_t j0=0;j0<5;j0++)
	  allData[i0][j0] = 0;

      if(start->Next()){
	for (Int_t i=0; i<105; i++) {
	  for(Int_t iHit=0;iHit<5;iHit++){
	    allData[i][iHit]= start->GetData(i,iHit);
	  }
	}
      }

      // Fill the histograms
      Float_t besttimeA=9999999;
      Float_t besttimeC=9999999;
      Float_t time[24];
       Float_t meanShift[24];
       for (Int_t ik = 0; ik<24; ik++)
	 {
	   if(ik<12 && allData[ik+1][0]>0
	      && (allData[ik+13][0]-allData[ik+1][0]) < 530 ){
	     hCFD1minCFD[ik]->Fill(allData[ik+1][0]-allData[npmtC][0]);
	   }

	   if(ik>11 && allData[ik+45][0]>0
	      && (allData[ik+57][0]-allData[ik+45][0]) <530 ){
	     hCFD1minCFD[ik]->Fill(allData[ik+45][0]-allData[56+npmtA][0]);
	   }
	   if(iev == 10000) {
	     meanShift[ik] =  hCFD1minCFD[ik]->GetMean();
	   }
	 }
      //fill  mean time _ fast reconstruction
      if (iev > 10000 )
	{
	  for (Int_t in=0; in<12; in++)
	    {
	      time[in] = allData[in+1][0] - meanShift[in]  ;
	      time[in+12] = allData[in+56+1][0] ;
	    }
	  for (Int_t ipmt=0; ipmt<12; ipmt++){
	    if(time[ipmt] > 1 ) {
	      if(time[ipmt]<besttimeC)
		besttimeC=time[ipmt]; //timeC
	    }
	  }
	   for ( Int_t ipmt=12; ipmt<24; ipmt++){
	     if(time[ipmt] > 1) {
	       if(time[ipmt]<besttimeA)
		 besttimeA=time[ipmt]; //timeA
	     }
	   }
	   if(besttimeA<9999999 &&besttimeC< 9999999) {
	     Float_t t0 =0.001* 24.4 * Float_t( besttimeA+besttimeC)/2.;
	     hVertex->Fill(t0);
	   }
	}

     delete start;
      start = 0x0;
      reader->Reset();
      // End of fill histograms

    }

    /* free resources */
    free(event);

    /* exit when last event received, no need to wait for TERM signal */
    if (eventT==END_OF_RUN) {
      printf("EOR event detected\n");
      printf("Number of events processed - %i\n ",iev);
      break;
    }
  }
  printf("After loop, before writing histos\n");
  // write a file with the histograms

  TFile *hist = new TFile(FILE_OUT,"RECREATE");

  for(Int_t j=0;j<24;j++){
     hCFD1minCFD[j]->Write();
    }
  hVertex->Write();
  hist->Close();
  delete hist;

  status=0;

  /* export file to FXS */
  if (daqDA_FES_storeFile(FILE_OUT, "PHYSICS")) {
    status=-2;
  }

  return status;
}

<commit_msg>DA with removing violations and tuned to coming data<commit_after>/*
T0 DA for online calibration

Contact: Michal.Oledzki@cern.ch
Link: http://users.jyu.fi/~mioledzk/
Run Type: PHYSICS
DA Type: MON
Number of events needed: 500000
Input Files: inPhys.dat, external parameters
Output Files: daPhys.root, to be exported to the DAQ FXS
Trigger types used: PHYSICS_EVENT
------------------------------Alla
Now trigger type changed to CALICRATION_EVENT
to have data during test.
SOULD BE CHANGED BACK BEFORE BEAM
------------------------------- Alla
*/

#define FILE_OUT "daPhys.root"
#define FILE_IN "inPhys.dat"
#include <daqDA.h>
#include <event.h>
#include <monitor.h>

#include <Riostream.h>
#include <stdio.h>
#include <stdlib.h>

//AliRoot
#include <AliRawReaderDate.h>
#include <AliRawReader.h>
#include <AliT0RawReader.h>

//ROOT
#include "TROOT.h"
#include "TPluginManager.h"
#include "TFile.h"
#include "TKey.h"
#include "TObject.h"
#include "TBenchmark.h"
#include "TString.h"
#include "TH1.h"
#include "TSpectrum.h"
#include "TMath.h"

int kbx, kcbx, kt0bx, knpmtA, knpmtC;
float klx,kmx,kclx,kcmx, kt0lx, kt0hx;

/* Main routine
      Arguments:
      1- monitoring data source
*/
int main(int argc, char **argv) {
//int main(){
  int status;

  /* magic line */
  gROOT->GetPluginManager()->AddHandler("TVirtualStreamerInfo",
                                        "*",
                                        "TStreamerInfo",
                                        "RIO",
                                        "TStreamerInfo()");

  if(daqDA_DB_getFile(FILE_IN, FILE_IN)){
    printf("Couldn't get input file >>inPhys.dat<< from DAQ_DB !!!\n");
    return -1;
  }

  FILE *inp;
  char c;
  inp = fopen(FILE_IN, "r");
  if(!inp){
    printf("Input file >>inPhys.dat<< not found !!!\n");
    return -1;
  }

  while((c=getc(inp))!=EOF) {
    switch(c) {
    case 'a': {fscanf(inp, "%d", &kcbx ); break;} //N of X bins hCFD1_CFD
    case 'b': {fscanf(inp, "%f", &kclx ); break;} //Low x hCFD1_CFD
    case 'c': {fscanf(inp, "%f", &kcmx ); break;} //High x hCFD1_CF
    case 'd': {fscanf(inp, "%d", &knpmtC ); break;} //number of reference PMTC
    case 'e': {fscanf(inp, "%d", &knpmtA ); break;} //number of reference PMTA
    case 'f': {fscanf(inp, "%d", &kt0bx ); break;} //N of X bins hT0
    case 'g': {fscanf(inp, "%f", &kt0lx ); break;} //Low x hT0
    case 'k': {fscanf(inp, "%f", &kt0hx ); break;} //High x hT0
    }
  }
  fclose(inp);

  if (argc!=2) {
    printf("Wrong number of arguments\n");
    return -1;
  }

  /* define data source : this is argument 1 */
  status=monitorSetDataSource( argv[1] );
  if (status!=0) {
    printf("monitorSetDataSource() failed : %s\n",monitorDecodeError(status));
    return -1;
  }

  /* declare monitoring program */
  status=monitorDeclareMp( __FILE__ );
  if (status!=0) {
    printf("monitorDeclareMp() failed : %s\n",monitorDecodeError(status));
    return -1;
  }

  /* define wait event timeout - 1s max */
  monitorSetNowait();
  monitorSetNoWaitNetworkTimeout(1000);

  /* log start of process */
  printf("T0 monitoring program started\n");

  // Allocation of histograms - start

  TH1F *hCFD1minCFD[24];

  for(Int_t ic=0; ic<24; ic++) {
    hCFD1minCFD[ic] = new TH1F(Form("CFD1minCFD%d",ic+1),"CFD-CFD",kcbx,kclx,kcmx);
  }
  TH1F *hVertex = new TH1F("hVertex","T0 time",kt0bx,kt0lx,kt0hx);

   // Allocation of histograms - end

  Int_t iev=0;
  /* main loop (infinite) */
  for(;;) {
    struct eventHeaderStruct *event;
    eventTypeType eventT;

    /* check shutdown condition */
    if (daqDA_checkShutdown()) {break;}

    /* get next event (blocking call until timeout) */
    status=monitorGetEventDynamic((void **)&event);
    if (status==(int)MON_ERR_EOF) {
      printf ("End of File detected\n");
      break; /* end of monitoring file has been reached */
    }

    if (status!=0) {
      printf("monitorGetEventDynamic() failed : %s\n",monitorDecodeError(status));
      break;
    }

    /* retry if got no event */
    if (event==NULL) {
      continue;
    }

    /* use event - here, just write event id to result file */
    eventT=event->eventType;

    switch (event->eventType){

      case START_OF_RUN:
	break;

    case END_OF_RUN:
      break;

        case PHYSICS_EVENT:
	  //    case CALIBRATION_EVENT:
      iev++;

      if(iev==1){
	printf("First event - %i\n",iev);
      }

      // Initalize raw-data reading and decoding
      AliRawReader *reader = new AliRawReaderDate((void*)event);

      // Enable the following two lines in case of real-data
      reader->RequireHeader(kTRUE);
      AliT0RawReader *start = new AliT0RawReader(reader, kTRUE);

      // Read raw data
      Int_t allData[105][5];
      for(Int_t i0=0;i0<105;i0++)
      	for(Int_t j0=0;j0<5;j0++)
	  allData[i0][j0] = 0;

      if(start->Next()){
	for (Int_t i=0; i<105; i++) {
	  for(Int_t iHit=0;iHit<5;iHit++){
	    allData[i][iHit]= start->GetData(i,iHit);
	  }
	}
      }

      // Fill the histograms
      Float_t besttimeA=9999999;
      Float_t besttimeC=9999999;
      Float_t time[24];
       Float_t meanShift[24];
       for (Int_t ik = 0; ik<24; ik++)
	 {
	   if(ik<12 && allData[ik+1][0]>0 && allData[knpmtC][0]>0 ){
	     hCFD1minCFD[ik]->Fill(allData[ik+1][0]-allData[knpmtC][0]);
	   }

	   if(ik>11 && allData[ik+45][0]>0 && allData[56+knpmtA][0]>0 )
	     {
	     hCFD1minCFD[ik]->Fill(allData[ik+45][0]-allData[56+knpmtA][0]);
	     }
	   if(iev == 10000) {
	     meanShift[ik] =  hCFD1minCFD[ik]->GetMean();
	     if(ik==knpmtC || ik==(56+knpmtA)) meanShift[ik]=0;
	   }
	 }
      //fill  mean time _ fast reconstruction
      if (iev > 10000 )
	{
	  for (Int_t in=0; in<12; in++)
	    {
	      time[in] = allData[in+1][0] - meanShift[in]  ;
	      time[in+12] = allData[in+56+1][0] ;
	    }
	  for (Int_t ipmt=0; ipmt<12; ipmt++){
	    if(time[ipmt] > 1 ) {
	      if(time[ipmt]<besttimeC)
		besttimeC=time[ipmt]; //timeC
	    }
	  }
	   for ( Int_t ipmt=12; ipmt<24; ipmt++){
	     if(time[ipmt] > 1) {
	       if(time[ipmt]<besttimeA)
		 besttimeA=time[ipmt]; //timeA
	     }
	   }
	   if(besttimeA<9999999 &&besttimeC< 9999999) {
	     Float_t t0 =0.001* 24.4 * Float_t( besttimeA+besttimeC)/2.;
	     hVertex->Fill(t0);
	   }
	}

     delete start;
      start = 0x0;
      reader->Reset();
      // End of fill histograms

    }

    /* free resources */
    free(event);

    /* exit when last event received, no need to wait for TERM signal */
    if (eventT==END_OF_RUN) {
      printf("EOR event detected\n");
      printf("Number of events processed - %i\n ",iev);
      break;
    }
  }
  printf("After loop, before writing histos\n");
  // write a file with the histograms

  TFile *hist = new TFile(FILE_OUT,"RECREATE");

  for(Int_t j=0;j<24;j++){
     hCFD1minCFD[j]->Write();
    }
  hVertex->Write();
  hist->Close();
  delete hist;

  status=0;

  /* export file to FXS */
  if (daqDA_FES_storeFile(FILE_OUT, "PHYSICS")) {
    status=-2;
  }

  return status;
}

<|endoftext|>


<commit_before>#include "src/graphics.h"
#include "src/math.h"

#include <stdlib.h>
#include <sys/timeb.h>

typedef struct
{
    Vector4f position;
    Vector4f target;
    Vector4f up;
    Vector4f right;
    Matrix4f view;
    Matrix4f projection;
} Camera;

// Projection test
void testProjection(int scrWidth, int scrHeight)
{
    unsigned short *keysPressed;
    unsigned int elapsed, i,x,y;
    struct timeb startTime, endTime;
    Vector4f vs, rs;
    Vector4f square[4];
    Camera cam;
    Matrix4f vp;
    unsigned char *buffer = (unsigned char *)malloc(scrWidth*scrHeight);

    cam.position.x = 0;
    cam.position.y = 0;
    cam.position.z = 1.f;
    cam.position.w = 1.f;

    cam.up.x = 0.f;
    cam.up.y = 1.f;
    cam.up.z = 0.f;
    cam.up.w = 1.f;

    cam.right.x = 1.f;
    cam.right.y = 0.f;
    cam.right.z = 0.f;
    cam.right.w = 1.f;

    cam.target.x = 0.f;
    cam.target.y = 0.f;
    cam.target.z = -1.f;
    cam.target.w = 1.f;

    if(!buffer)
    {
        printf("Out of memory!");
        return;
    }

    keysPressed = translateInput();

    while(!keysPressed[KEY_ESC])
    {
        for(i = 0; i < 4; i++)
        {
            square[i].x = 0 + 50*(i%2);
            square[i].y = 0 + 50*(i > 1 ? 1 : 0);
            square[i].z = -8.f;
            square[i].w = 1.f;
        }

        matView(&cam.view, &cam.position, &cam.target, &cam.up);
        matPerspective(&cam.projection, 75.f * M_PI /180.f, (float)scrWidth / (float)scrHeight, 0.1f, 5.f);
        vp = matMul(&cam.view, &cam.projection);
        matTranspose(&vp);

        for(i = 0; i < 4; i++)
        {
            square[i] = matMulVec(&vp, &square[i]);
            square[i].x /= square[i].w;
            square[i].y /= square[i].w;
            square[i].z /= square[i].w;

            square[i].x = (square[i].x * (float)scrWidth)  / (2.0f * square[i].w) + (scrWidth >> 1);
            square[i].y = (square[i].y * (float)scrHeight) / (2.0f * square[i].w) + (scrHeight >> 1);
            //printf("%.2f %.2f %.2f %.2f\n", square[i].x, square[i].y, square[i].z, square[i].w);
        }

        clrScrBuffer(buffer);

        drawLine(square[0].x, square[0].y, square[1].x, square[1].y, 3, buffer);
        drawLine(square[1].x, square[1].y, square[3].x, square[3].y, 3, buffer);
        drawLine(square[0].x, square[0].y, square[2].x, square[2].y, 3, buffer);
        drawLine(square[2].x, square[2].y, square[3].x, square[3].y, 3, buffer);

        updateScreen(buffer);

        keysPressed = translateInput();

        vs = vecScale(&cam.target, 0.01f);
        rs = vecScale(&cam.right, 0.1f);
        if(keysPressed[KEY_W])
            cam.position = vecAdd(&cam.position, &vs);

        if(keysPressed[KEY_S])
            cam.position = vecSub(&cam.position, &vs);

        if(keysPressed[KEY_A])
            cam.position = vecSub(&cam.position, &rs);

        if(keysPressed[KEY_D])
            cam.position = vecAdd(&cam.position, &rs);

        if(keysPressed[KEY_LEFT])
        {
            rotateVecAxisAngle(&cam.target, 0.0001f, cam.up.x, cam.up.y, cam.up.z);
            cam.right = crossProduct(&cam.target, &cam.up);
        }

        if(keysPressed[KEY_RIGHT])
        {
            rotateVecAxisAngle(&cam.target, -0.0001f, cam.up.x, cam.up.y, cam.up.z);
            cam.right = crossProduct(&cam.target, &cam.up);
        }
    }

    ftime(&startTime);
    ftime(&endTime);

    free(buffer);

    elapsed = (endTime.time - startTime.time)*1000 + endTime.millitm - startTime.millitm;

    do {
        keysPressed = translateInput();
    } while(keysPressed[KEY_ESC]);
}
<commit_msg>fixed perspective problems<commit_after>#include "src/graphics.h"
#include "src/math.h"

#include <stdlib.h>
#include <sys/timeb.h>

typedef struct
{
    Vector4f position;
    Vector4f target;
    Vector4f up;
    Vector4f right;
    Matrix4f view;
    Matrix4f projection;
} Camera;

// Projection test
void testProjection(int scrWidth, int scrHeight)
{
    unsigned short *keysPressed;
    unsigned int elapsed, i,x,y;
    struct timeb startTime, endTime;
    Vector4f vs, rs;
    Vector4f square[4];
    Camera cam;
    Matrix4f vp;
    unsigned char *buffer = (unsigned char *)malloc(scrWidth*scrHeight);

    cam.position.x = 0;
    cam.position.y = 0;
    cam.position.z = 1.f;
    cam.position.w = 1.f;

    cam.up.x = 0.f;
    cam.up.y = 1.f;
    cam.up.z = 0.f;
    cam.up.w = 1.f;

    cam.right.x = 1.f;
    cam.right.y = 0.f;
    cam.right.z = 0.f;
    cam.right.w = 1.f;

    cam.target.x = 0.f;
    cam.target.y = 0.f;
    cam.target.z = -1.f;
    cam.target.w = 1.f;

    if(!buffer)
    {
        printf("Out of memory!");
        return;
    }

    keysPressed = translateInput();

    while(!keysPressed[KEY_ESC])
    {
        for(i = 0; i < 4; i++)
        {
            square[i].x = 0 + 50*(i%2);
            square[i].y = 0 + 50*(i > 1 ? 1 : 0);
            square[i].z = -80.f;
            square[i].w = 1.f;
        }

        matView(&cam.view, &cam.position, &cam.target, &cam.up);
        matPerspective(&cam.projection, 75.f * M_PI /180.f, (float)scrWidth / (float)scrHeight, 0.1f, 5.f);
        vp = matMul(&cam.view, &cam.projection);
        matTranspose(&vp);

        for(i = 0; i < 4; i++)
        {
            square[i] = matMulVec(&vp, &square[i]);

            // translate position to screen pixels
            square[i].x = (square[i].x * (float)scrWidth)  / (2.0f * square[i].w) + (scrWidth >> 1);
            square[i].y = (square[i].y * (float)scrHeight) / (2.0f * square[i].w) + (scrHeight >> 1);
        }

        clrScrBuffer(buffer);

        drawLine(square[0].x, square[0].y, square[1].x, square[1].y, 3, buffer);
        drawLine(square[1].x, square[1].y, square[3].x, square[3].y, 3, buffer);
        drawLine(square[0].x, square[0].y, square[2].x, square[2].y, 3, buffer);
        drawLine(square[2].x, square[2].y, square[3].x, square[3].y, 3, buffer);

        updateScreen(buffer);

        keysPressed = translateInput();

        vs = vecScale(&cam.target, 0.1f);
        rs = vecScale(&cam.right, 0.1f);

        if(keysPressed[KEY_W])
            cam.position = vecAdd(&cam.position, &vs);

        if(keysPressed[KEY_S])
            cam.position = vecSub(&cam.position, &vs);

        if(keysPressed[KEY_A])
            cam.position = vecSub(&cam.position, &rs);

        if(keysPressed[KEY_D])
            cam.position = vecAdd(&cam.position, &rs);

        if(keysPressed[KEY_LEFT])
        {
            rotateVecAxisAngle(&cam.target, 0.001f, cam.up.x, cam.up.y, cam.up.z);
            cam.right = crossProduct(&cam.target, &cam.up);
        }

        if(keysPressed[KEY_RIGHT])
        {
            rotateVecAxisAngle(&cam.target, -0.001f, cam.up.x, cam.up.y, cam.up.z);
            cam.right = crossProduct(&cam.target, &cam.up);
        }

        if(keysPressed[KEY_PGUP])
        {
            rotateVecAxisAngle(&cam.target, -0.001f, cam.right.x, cam.right.y, cam.right.z);
            cam.right = crossProduct(&cam.target, &cam.up);
        }

        if(keysPressed[KEY_PGDN])
        {
            rotateVecAxisAngle(&cam.target, 0.001f, cam.right.x, cam.right.y, cam.right.z);
            cam.up = crossProduct(&cam.right, &cam.target);
        }
    }

    ftime(&startTime);
    ftime(&endTime);

    free(buffer);

    elapsed = (endTime.time - startTime.time)*1000 + endTime.millitm - startTime.millitm;

    do {
        keysPressed = translateInput();
    } while(keysPressed[KEY_ESC]);
}
<|endoftext|>


<commit_before>#ifndef _CCTAG_DISTANCE_HPP_
#define _CCTAG_DISTANCE_HPP_

#include <cctag/geometry/Ellipse.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/special_functions/pow.hpp>
#include <boost/units/cmath.hpp>
#include <boost/foreach.hpp>
#include <boost/math/special_functions/pow.hpp>

namespace cctag {
namespace numerical {

namespace ublas = boost::numeric::ublas;

template<class T>
inline double distancePoints2D( const T& p1, const T& p2 ) // TODO modifier les accès, considérer p1, p2 comme des bounded_vector
{
	return std::sqrt( (double)boost::math::pow<2>( p2.x() - p1.x() ) +
	                  boost::math::pow<2>( p2.y() - p1.y() ) );
}

template<class T>
inline double powDistancePoints2D( const T& p1, const T& p2 ) // TODO modifier les accès, considérer p1, p2 comme des bounded_vector
{
	return boost::math::pow<2>( p2.x() - p1.x() ) +
	       boost::math::pow<2>( p2.y() - p1.y() );
}

template<class T>
inline double distancePoints3D( const T& p1, const T& p2 ) // TODO modifier les accès, considérer p1, p2 comme des bounded_vector
{
	return std::sqrt( (double)( p2.x() - p1.x() ) * ( p2.x() - p1.x() ) +
	                  ( p2.y() - p1.y() ) * ( p2.y() - p1.y() ) +
	                  ( p2.z() - p1.z() ) * ( p2.z() - p1.z() ) );
}

// Compute (point-polar) distance between a point and an ellipse represented by its 3x3 matrix.
inline double distancePointEllipse( const ublas::bounded_vector<double, 3>& p, const ublas::bounded_matrix<double, 3, 3> & Q, const double f )
{
	ublas::bounded_vector<double, 6> aux( 6 );

	aux( 0 ) = p( 0 ) * p( 0 );
	aux( 1 ) = 2 * p( 0 ) * p( 1 );
	aux( 2 ) = 2* f* p( 0 );
	aux( 3 ) = p( 1 ) * p( 1 );
	aux( 4 ) = 2* f* p( 1 );
	aux( 5 ) = f * f;

	//sdist = ([pts(:,1).*pts(:,1) 2*pts(:,1).*pts(:,2) pts(:,2).*pts(:,2) 2*f*pts(:,1) 2*f*pts(:,2) f*f*ones(n,1)]*Q([1;2;5;7;8;9])).^2./((pts*Q([1;2;3])).^2+(pts*Q([2;5;8])).^2);
	double tmp1  = p( 0 ) * Q( 0, 0 ) + p( 1 ) * Q( 0, 1 ) + p( 2 ) * Q( 0, 2 );
	double tmp2  = p( 0 ) * Q( 0, 1 ) + p( 1 ) * Q( 1, 1 ) + p( 2 ) * Q( 1, 2 );
	double denom = tmp1 * tmp1 + tmp2 * tmp2;

	ublas::bounded_vector<double, 6> qL;
	qL( 0 ) = Q( 0, 0 ) ; qL( 1 ) = Q( 0, 1 ) ; qL( 2 ) = Q( 0, 2 ) ;
	qL( 3 ) = Q( 1, 1 ) ; qL( 4 ) = Q( 1, 2 ) ;
	qL( 5 ) = Q( 2, 2 );
	return boost::math::pow<2>( ublas::inner_prod( aux, qL ) ) / denom;
}

inline double distancePointEllipse( const ublas::bounded_vector<double, 3>& p, const geometry::Ellipse& q, const double f )
{
	const ublas::bounded_matrix<double, 3, 3>& Q = q.matrix();

	return distancePointEllipse( p, Q, f );
}

// Compute the distance between points and an ellipse
//template<class T>
inline void distancePointEllipse( std::vector<double>& dist, const std::vector<ublas::bounded_vector<double, 3> >& pts, const geometry::Ellipse& q, const double f )
{
	dist.clear();
	dist.reserve( pts.size() );
	typedef ublas::bounded_vector<double, 3> Point;

	BOOST_FOREACH( const Point &p, pts )
	{
		dist.push_back( distancePointEllipse( p, q, f ) );
	}
}

}
}

#endif
<commit_msg>Two template arguments for point to point distance.<commit_after>#ifndef _CCTAG_DISTANCE_HPP_
#define _CCTAG_DISTANCE_HPP_

#include <cctag/geometry/Ellipse.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/special_functions/pow.hpp>
#include <boost/units/cmath.hpp>
#include <boost/foreach.hpp>
#include <boost/math/special_functions/pow.hpp>

namespace cctag {
namespace numerical {

namespace ublas = boost::numeric::ublas;

template<class T, class U>
inline double distancePoints2D( const T& p1, const U& p2 ) // TODO modifier les accès, considérer p1, p2 comme des bounded_vector
{
	return std::sqrt( (double)boost::math::pow<2>( p2.x() - p1.x() ) +
	                  boost::math::pow<2>( p2.y() - p1.y() ) );
}

template<class T>
inline double powDistancePoints2D( const T& p1, const T& p2 ) // TODO modifier les accès, considérer p1, p2 comme des bounded_vector
{
	return boost::math::pow<2>( p2.x() - p1.x() ) +
	       boost::math::pow<2>( p2.y() - p1.y() );
}

template<class T>
inline double distancePoints3D( const T& p1, const T& p2 ) // TODO modifier les accès, considérer p1, p2 comme des bounded_vector
{
	return std::sqrt( (double)( p2.x() - p1.x() ) * ( p2.x() - p1.x() ) +
	                  ( p2.y() - p1.y() ) * ( p2.y() - p1.y() ) +
	                  ( p2.z() - p1.z() ) * ( p2.z() - p1.z() ) );
}

// Compute (point-polar) distance between a point and an ellipse represented by its 3x3 matrix.
inline double distancePointEllipse( const ublas::bounded_vector<double, 3>& p, const ublas::bounded_matrix<double, 3, 3> & Q, const double f )
{
	ublas::bounded_vector<double, 6> aux( 6 );

	aux( 0 ) = p( 0 ) * p( 0 );
	aux( 1 ) = 2 * p( 0 ) * p( 1 );
	aux( 2 ) = 2* f* p( 0 );
	aux( 3 ) = p( 1 ) * p( 1 );
	aux( 4 ) = 2* f* p( 1 );
	aux( 5 ) = f * f;

	//sdist = ([pts(:,1).*pts(:,1) 2*pts(:,1).*pts(:,2) pts(:,2).*pts(:,2) 2*f*pts(:,1) 2*f*pts(:,2) f*f*ones(n,1)]*Q([1;2;5;7;8;9])).^2./((pts*Q([1;2;3])).^2+(pts*Q([2;5;8])).^2);
	double tmp1  = p( 0 ) * Q( 0, 0 ) + p( 1 ) * Q( 0, 1 ) + p( 2 ) * Q( 0, 2 );
	double tmp2  = p( 0 ) * Q( 0, 1 ) + p( 1 ) * Q( 1, 1 ) + p( 2 ) * Q( 1, 2 );
	double denom = tmp1 * tmp1 + tmp2 * tmp2;

	ublas::bounded_vector<double, 6> qL;
	qL( 0 ) = Q( 0, 0 ) ; qL( 1 ) = Q( 0, 1 ) ; qL( 2 ) = Q( 0, 2 ) ;
	qL( 3 ) = Q( 1, 1 ) ; qL( 4 ) = Q( 1, 2 ) ;
	qL( 5 ) = Q( 2, 2 );
	return boost::math::pow<2>( ublas::inner_prod( aux, qL ) ) / denom;
}

inline double distancePointEllipse( const ublas::bounded_vector<double, 3>& p, const geometry::Ellipse& q, const double f )
{
	const ublas::bounded_matrix<double, 3, 3>& Q = q.matrix();

	return distancePointEllipse( p, Q, f );
}

// Compute the distance between points and an ellipse
//template<class T>
inline void distancePointEllipse( std::vector<double>& dist, const std::vector<ublas::bounded_vector<double, 3> >& pts, const geometry::Ellipse& q, const double f )
{
	dist.clear();
	dist.reserve( pts.size() );
	typedef ublas::bounded_vector<double, 3> Point;

	BOOST_FOREACH( const Point &p, pts )
	{
		dist.push_back( distancePointEllipse( p, q, f ) );
	}
}

}
}

#endif
<|endoftext|>


<commit_before>// Copyright 2018 The Ripple Authors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "init.h"

namespace mysql_ripple {

void Init(int argc, char** argv) {
}

}  // namespace mysql_ripple
<commit_msg>Fix parsing of flags<commit_after>// Copyright 2018 The Ripple Authors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "init.h"
#include "flags.h"

namespace mysql_ripple {

void Init(int argc, char** argv) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
}

}  // namespace mysql_ripple
<|endoftext|>


<commit_before>/****************** <VPR heading BEGIN do not edit this line> *****************
 *
 * VR Juggler Portable Runtime
 *
 * Original Authors:
 *   Allen Bierbaum, Patrick Hartling, Kevin Meinert, Carolina Cruz-Neira
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 ****************** <VPR heading END do not edit this line> ******************/

/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998, 1999, 2000, 2001 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vpr/vprConfig.h>

#ifdef VPR_SIMULATOR
#  include <vpr/md/SIM/Controller.h>
#endif

#include <vpr/Util/Interval.h>
#include <vpr/System.h>

const vpr::Interval vpr::Interval::NoWait(0,vpr::Interval::Base);
const vpr::Interval vpr::Interval::NoTimeout(0xffffffffUL, vpr::Interval::Base);
const vpr::Interval vpr::Interval::HalfPeriod((0xffffffffUL/2), vpr::Interval::Base);

namespace vpr
{

   // Simulator-only version of vpr::Interval::setNow().
#ifdef VPR_SIMULATOR
void Interval::setNow()
{
   mMicroSeconds = vpr::sim::Controller::instance()->getClock().getCurrentTime().getBaseVal();
}
#else
void Interval::setNow()
{
   setNowReal();
}
#endif /* ifdef VPR_SIMULATOR */

//
// Real implementation of setNow that uses the real clock time from the system
//
void Interval::setNowReal()
{
#if defined(VPR_OS_Win32)
   LARGE_INTEGER count;
   LARGE_INTEGER counts_per_sec;

   QueryPerformanceFrequency(&counts_per_sec);

   vpr::Uint64 counts_per_sec64;
   vpr::Uint64 counts_per_sec_high64;

   counts_per_sec_high64 = counts_per_sec.HighPart;
   counts_per_sec64 = counts_per_sec.LowPart;
   counts_per_sec64 += (counts_per_sec_high64 << 32);

   // XXX: Implement this
   /* Sadly; nspr requires the interval to range from 1000 ticks per second
    * to only 100000 ticks per second; QueryPerformanceCounter is too high
    * resolution...
    */
   if (QueryPerformanceCounter(&count))
   {
      const vpr::Uint64 microseconds_per_sec = 1000000u;

      vpr::Uint64 low  = count.LowPart;
      vpr::Uint64 high = count.HighPart;
      mMicroSeconds = low + (high << 32);
      mMicroSeconds /= counts_per_sec64;
      mMicroSeconds *= microseconds_per_sec;

//      vpr::Int32 top = count.HighPart & _nt_highMask;
//      top = top << (32 - _nt_bitShift);
//      count.LowPart = count.LowPart >> _nt_bitShift;
//      count.LowPart = count.LowPart + top;
//      mMicroSeconds = count.LowPart;
   }
/*
   else
   {
#if defined(__MINGW32__)
      mMicroSeconds = time();
#elif defined(WIN16)
      mMicroSeconds = clock();        // milliseconds since application start
#else
      mMicroSeconds = GetTickCount();  // milliseconds since system start
#endif
   }
*/
#else    // Default to POSIX time setting

   timeval cur_time;
   vpr::System::gettimeofday(&cur_time);
   mMicroSeconds = (cur_time.tv_usec + (1000000 * cur_time.tv_sec));

#endif
}

}; // namespace vpr

<commit_msg>Removed code copied from NSPR that we commented out.<commit_after>/****************** <VPR heading BEGIN do not edit this line> *****************
 *
 * VR Juggler Portable Runtime
 *
 * Original Authors:
 *   Allen Bierbaum, Patrick Hartling, Kevin Meinert, Carolina Cruz-Neira
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 ****************** <VPR heading END do not edit this line> ******************/

/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998, 1999, 2000, 2001 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

#include <vpr/vprConfig.h>

#ifdef VPR_SIMULATOR
#  include <vpr/md/SIM/Controller.h>
#endif

#include <vpr/Util/Interval.h>
#include <vpr/System.h>

const vpr::Interval vpr::Interval::NoWait(0,vpr::Interval::Base);
const vpr::Interval vpr::Interval::NoTimeout(0xffffffffUL, vpr::Interval::Base);
const vpr::Interval vpr::Interval::HalfPeriod((0xffffffffUL/2), vpr::Interval::Base);

namespace vpr
{

   // Simulator-only version of vpr::Interval::setNow().
#ifdef VPR_SIMULATOR
void Interval::setNow()
{
   mMicroSeconds = vpr::sim::Controller::instance()->getClock().getCurrentTime().getBaseVal();
}
#else
void Interval::setNow()
{
   setNowReal();
}
#endif /* ifdef VPR_SIMULATOR */

//
// Real implementation of setNow that uses the real clock time from the system
//
void Interval::setNowReal()
{
#if defined(VPR_OS_Win32)
   LARGE_INTEGER count;
   LARGE_INTEGER counts_per_sec;

   QueryPerformanceFrequency(&counts_per_sec);

   vpr::Uint64 counts_per_sec64;
   vpr::Uint64 counts_per_sec_high64;

   counts_per_sec_high64 = counts_per_sec.HighPart;
   counts_per_sec64 = counts_per_sec.LowPart;
   counts_per_sec64 += (counts_per_sec_high64 << 32);

   // XXX: Implement this
   /* Sadly; nspr requires the interval to range from 1000 ticks per second
    * to only 100000 ticks per second; QueryPerformanceCounter is too high
    * resolution...
    */
   if (QueryPerformanceCounter(&count))
   {
      const vpr::Uint64 microseconds_per_sec = 1000000u;

      vpr::Uint64 low  = count.LowPart;
      vpr::Uint64 high = count.HighPart;
      mMicroSeconds = low + (high << 32);
      mMicroSeconds /= counts_per_sec64;
      mMicroSeconds *= microseconds_per_sec;
   }
/*
   else
   {
#if defined(__MINGW32__)
      mMicroSeconds = time();
#elif defined(WIN16)
      mMicroSeconds = clock();        // milliseconds since application start
#else
      mMicroSeconds = GetTickCount();  // milliseconds since system start
#endif
   }
*/
#else    // Default to POSIX time setting

   timeval cur_time;
   vpr::System::gettimeofday(&cur_time);
   mMicroSeconds = (cur_time.tv_usec + (1000000 * cur_time.tv_sec));

#endif
}

}; // namespace vpr

<|endoftext|>


<commit_before>/**
 * This file is part of the "FnordMetric" project
 *   Copyright (c) 2014 Paul Asmuth, Google Inc.
 *
 * FnordMetric is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License v3.0. You should have received a
 * copy of the GNU General Public License along with this program. If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include <stx/io/BufferedOutputStream.h>
#include <stx/io/fileutil.h>
#include <chartsql/runtime/groupby.h>

namespace csql {

GroupBy::GroupBy(
    ScopedPtr<TableExpression> source,
    const Vector<String>& column_names,
    Vector<ScopedPtr<ValueExpression>> select_expressions,
    Vector<ScopedPtr<ValueExpression>> group_expressions,
    SHA1Hash qtree_fingerprint) :
    source_(std::move(source)),
    column_names_(column_names),
    select_exprs_(std::move(select_expressions)),
    group_exprs_(std::move(group_expressions)),
    qtree_fingerprint_(qtree_fingerprint) {}

void GroupBy::execute(
    ExecutionContext* context,
    Function<bool (int argc, const SValue* argv)> fn) {
  HashMap<String, Vector<VM::Instance >> groups;
  ScratchMemory scratch;

  try {
    accumulate(&groups, &scratch, context);
    getResult(&groups, fn);
  } catch (...) {
    freeResult(&groups);
    throw;
  }

  freeResult(&groups);
}

void GroupBy::accumulate(
    HashMap<String, Vector<VM::Instance >>* groups,
    ScratchMemory* scratch,
    ExecutionContext* context) {
  auto cache_key = cacheKey();
  String cache_filename;
  bool from_cache = false;
  auto cachedir = context->cacheDir();
  if (!cache_key.isEmpty() && !cachedir.isEmpty()) {
    cache_filename = FileUtil::joinPaths(
        cachedir.get(),
        cache_key.get().toString() + ".qcache");

    if (FileUtil::exists(cache_filename)) {
      auto fis = FileInputStream::openFile(cache_filename);
      from_cache = decode(groups, scratch, fis.get());
    }
  }

  if (!from_cache) {
    source_->execute(
        context,
        std::bind(
            &GroupBy::nextRow,
            this,
            groups,
            scratch,
            std::placeholders::_1,
            std::placeholders::_2));
  }

  if (!from_cache && !cache_key.isEmpty() && !cachedir.isEmpty()) {
    BufferedOutputStream fos(
        FileOutputStream::fromFile(
            File::openFile(
                cache_filename + "~",
                File::O_CREATEOROPEN | File::O_WRITE | File::O_TRUNCATE)));

    encode(groups, &fos);
    FileUtil::mv(cache_filename + "~", cache_filename);
  }
}

void GroupBy::getResult(
    const HashMap<String, Vector<VM::Instance >>* groups,
    Function<bool (int argc, const SValue* argv)> fn) {
  Vector<SValue> out_row(select_exprs_.size(), SValue{});
  for (auto& group : *groups) {
    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      select_exprs_[i]->result(&group.second[i], &out_row[i]);
    }

    if (!fn(out_row.size(), out_row.data())) {
      return;
    }
  }
}

void GroupBy::freeResult(
    HashMap<String, Vector<VM::Instance >>* groups) {
  for (auto& group : (*groups)) {
    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      select_exprs_[i]->freeInstance(&group.second[i]);
    }
  }
}

void GroupBy::mergeResult(
    const HashMap<String, Vector<VM::Instance >>* src,
    HashMap<String, Vector<VM::Instance >>* dst,
    ScratchMemory* scratch) {
  for (const auto& src_group : *src) {
    auto& dst_group = (*dst)[src_group.first];
    if (dst_group.size() == 0) {
      for (const auto& e : select_exprs_) {
        dst_group.emplace_back(e->allocInstance(scratch));
      }
    }

    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      select_exprs_[i]->merge(&dst_group[i], &src_group.second[i]);
    }
  }
}

bool GroupBy::nextRow(
    HashMap<String, Vector<VM::Instance >>* groups,
    ScratchMemory* scratch,
    int row_len, const SValue* row) {
  Vector<SValue> gkey(group_exprs_.size(), SValue{});
  for (size_t i = 0; i < group_exprs_.size(); ++i) {
    group_exprs_[i]->evaluate(row_len, row, &gkey[i]);
  }

  auto group_key = SValue::makeUniqueKey(gkey.data(), gkey.size());
  auto& group = (*groups)[group_key];
  if (group.size() == 0) {
    for (const auto& e : select_exprs_) {
      group.emplace_back(e->allocInstance(scratch));
    }
  }

  for (size_t i = 0; i < select_exprs_.size(); ++i) {
    select_exprs_[i]->accumulate(&group[i], row_len, row);
  }

  return true;
}

Vector<String> GroupBy::columnNames() const {
  return column_names_;
}

size_t GroupBy::numColumns() const {
  return column_names_.size();
}

void GroupBy::encode(
    const HashMap<String, Vector<VM::Instance >>* groups,
    OutputStream* os) const {
  os->appendVarUInt(groups->size());
  os->appendVarUInt(select_exprs_.size());

  for (auto& group : *groups) {
    os->appendLenencString(group.first);

    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      select_exprs_[i]->saveState(&group.second[i], os);
    }
  }
}

bool GroupBy::decode(
    HashMap<String, Vector<VM::Instance >>* groups,
    ScratchMemory* scratch,
    InputStream* is) const {
  auto ngroups = is->readVarUInt();
  auto nexprs = is->readVarUInt();

  if (select_exprs_.size() != nexprs) {
    return false;
  }

  for (size_t j = 0; j < ngroups; ++j) {
    auto group_key = is->readLenencString();

    auto& group = (*groups)[group_key];
    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      const auto& e = select_exprs_[i];
      group.emplace_back(e->allocInstance(scratch));
      e->loadState(&group[i], is);
    }
  }

  return true;
}

Option<SHA1Hash> GroupBy::cacheKey() const {
  auto source_key = source_->cacheKey();
  if (source_key.isEmpty()) {
    return None<SHA1Hash>();
  }

  return Some(
      SHA1::compute(
          StringUtil::format(
              "$0~$1",
              source_key.get().toString(),
              qtree_fingerprint_.toString())));
}

}
<commit_msg>use new VM iface in GroupBy<commit_after>/**
 * This file is part of the "FnordMetric" project
 *   Copyright (c) 2014 Paul Asmuth, Google Inc.
 *
 * FnordMetric is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License v3.0. You should have received a
 * copy of the GNU General Public License along with this program. If not, see
 * <http://www.gnu.org/licenses/>.
 */
#include <stx/io/BufferedOutputStream.h>
#include <stx/io/fileutil.h>
#include <chartsql/runtime/groupby.h>

namespace csql {

GroupBy::GroupBy(
    ScopedPtr<TableExpression> source,
    const Vector<String>& column_names,
    Vector<ScopedPtr<ValueExpression>> select_expressions,
    Vector<ScopedPtr<ValueExpression>> group_expressions,
    SHA1Hash qtree_fingerprint) :
    source_(std::move(source)),
    column_names_(column_names),
    select_exprs_(std::move(select_expressions)),
    group_exprs_(std::move(group_expressions)),
    qtree_fingerprint_(qtree_fingerprint) {}

void GroupBy::execute(
    ExecutionContext* context,
    Function<bool (int argc, const SValue* argv)> fn) {
  HashMap<String, Vector<VM::Instance >> groups;
  ScratchMemory scratch;

  try {
    accumulate(&groups, &scratch, context);
    getResult(&groups, fn);
  } catch (...) {
    freeResult(&groups);
    throw;
  }

  freeResult(&groups);
}

void GroupBy::accumulate(
    HashMap<String, Vector<VM::Instance >>* groups,
    ScratchMemory* scratch,
    ExecutionContext* context) {
  auto cache_key = cacheKey();
  String cache_filename;
  bool from_cache = false;
  auto cachedir = context->cacheDir();
  if (!cache_key.isEmpty() && !cachedir.isEmpty()) {
    cache_filename = FileUtil::joinPaths(
        cachedir.get(),
        cache_key.get().toString() + ".qcache");

    if (FileUtil::exists(cache_filename)) {
      auto fis = FileInputStream::openFile(cache_filename);
      from_cache = decode(groups, scratch, fis.get());
    }
  }

  if (!from_cache) {
    source_->execute(
        context,
        std::bind(
            &GroupBy::nextRow,
            this,
            groups,
            scratch,
            std::placeholders::_1,
            std::placeholders::_2));
  }

  if (!from_cache && !cache_key.isEmpty() && !cachedir.isEmpty()) {
    BufferedOutputStream fos(
        FileOutputStream::fromFile(
            File::openFile(
                cache_filename + "~",
                File::O_CREATEOROPEN | File::O_WRITE | File::O_TRUNCATE)));

    encode(groups, &fos);
    FileUtil::mv(cache_filename + "~", cache_filename);
  }
}

void GroupBy::getResult(
    const HashMap<String, Vector<VM::Instance >>* groups,
    Function<bool (int argc, const SValue* argv)> fn) {
  Vector<SValue> out_row(select_exprs_.size(), SValue{});
  for (auto& group : *groups) {
    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      VM::result(select_exprs_[i]->program(), &group.second[i], &out_row[i]);
    }

    if (!fn(out_row.size(), out_row.data())) {
      return;
    }
  }
}

void GroupBy::freeResult(
    HashMap<String, Vector<VM::Instance >>* groups) {
  for (auto& group : (*groups)) {
    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      VM::freeInstance(select_exprs_[i]->program(), &group.second[i]);
    }
  }
}

void GroupBy::mergeResult(
    const HashMap<String, Vector<VM::Instance >>* src,
    HashMap<String, Vector<VM::Instance >>* dst,
    ScratchMemory* scratch) {
  for (const auto& src_group : *src) {
    auto& dst_group = (*dst)[src_group.first];
    if (dst_group.size() == 0) {
      for (const auto& e : select_exprs_) {
        dst_group.emplace_back(VM::allocInstance(e->program(), scratch));
      }
    }

    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      VM::merge(
          select_exprs_[i]->program(),
          &dst_group[i],
          &src_group.second[i]);
    }
  }
}

bool GroupBy::nextRow(
    HashMap<String, Vector<VM::Instance >>* groups,
    ScratchMemory* scratch,
    int row_len, const SValue* row) {
  Vector<SValue> gkey(group_exprs_.size(), SValue{});
  for (size_t i = 0; i < group_exprs_.size(); ++i) {
    VM::evaluate(group_exprs_[i]->program(), row_len, row, &gkey[i]);
  }

  auto group_key = SValue::makeUniqueKey(gkey.data(), gkey.size());
  auto& group = (*groups)[group_key];
  if (group.size() == 0) {
    for (const auto& e : select_exprs_) {
      group.emplace_back(VM::allocInstance(e->program(), scratch));
    }
  }

  for (size_t i = 0; i < select_exprs_.size(); ++i) {
    VM::accumulate(select_exprs_[i]->program(), &group[i], row_len, row);
  }

  return true;
}

Vector<String> GroupBy::columnNames() const {
  return column_names_;
}

size_t GroupBy::numColumns() const {
  return column_names_.size();
}

void GroupBy::encode(
    const HashMap<String, Vector<VM::Instance >>* groups,
    OutputStream* os) const {
  os->appendVarUInt(groups->size());
  os->appendVarUInt(select_exprs_.size());

  for (auto& group : *groups) {
    os->appendLenencString(group.first);

    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      VM::saveState(select_exprs_[i]->program(), &group.second[i], os);
    }
  }
}

bool GroupBy::decode(
    HashMap<String, Vector<VM::Instance >>* groups,
    ScratchMemory* scratch,
    InputStream* is) const {
  auto ngroups = is->readVarUInt();
  auto nexprs = is->readVarUInt();

  if (select_exprs_.size() != nexprs) {
    return false;
  }

  for (size_t j = 0; j < ngroups; ++j) {
    auto group_key = is->readLenencString();

    auto& group = (*groups)[group_key];
    for (size_t i = 0; i < select_exprs_.size(); ++i) {
      const auto& e = select_exprs_[i];
      group.emplace_back(VM::allocInstance(e->program(), scratch));
      VM::loadState(e->program(), &group[i], is);
    }
  }

  return true;
}

Option<SHA1Hash> GroupBy::cacheKey() const {
  auto source_key = source_->cacheKey();
  if (source_key.isEmpty()) {
    return None<SHA1Hash>();
  }

  return Some(
      SHA1::compute(
          StringUtil::format(
              "$0~$1",
              source_key.get().toString(),
              qtree_fingerprint_.toString())));
}

}
<|endoftext|>