<commit_before>/*
Copyright (C) 2011 by Ivan Safrin

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#define PI 3.14159265

#include "PolyPhysicsScreenEntity.h"
#include "PolyLogger.h"
#include "PolyMesh.h"
#include "PolyPolygon.h"
#include "PolyScreenEntity.h"
#include "PolyScreenMesh.h"

using namespace Polycode;

PhysicsScreenEntity::PhysicsScreenEntity(ScreenEntity *entity, b2World *world, Number worldScale, int entType, bool isStatic, Number friction, Number density, Number restitution, bool isSensor, bool fixedRotation, int groupIndex) {

	screenEntity = entity;

	Vector3 entityScale = entity->getCompoundScale();
	Matrix4 compoundMatrix = screenEntity->getConcatenatedMatrix();
	entity->ignoreParentMatrix = true;
	entity->scale = entityScale;
	this->worldScale = worldScale;
	collisionOnly = false;

	// Create body definition---------------------------------------
	b2BodyDef bodyDef;
	bodyDef.position.Set(compoundMatrix.getPosition().x/worldScale, compoundMatrix.getPosition().y/worldScale);
	bodyDef.angle = screenEntity->getRotation()*(PI/180.0f);
	bodyDef.bullet = isSensor;
	bodyDef.fixedRotation = fixedRotation;
	if(isStatic)
		bodyDef.type = b2_staticBody;
	else
		bodyDef.type = b2_dynamicBody;

	// Create the body
	body = world->CreateBody(&bodyDef);
	body->SetUserData(this);

	// Create fixture definition---------------------------------------------
	b2FixtureDef fDef;
	fDef.friction = friction;
	fDef.restitution = restitution;
	fDef.density = density;
	fDef.isSensor = isSensor;
	fDef.filter.groupIndex = groupIndex;

	// Create Shape definition (Circle/Rectangle/Polygon)---------------------------
	switch(entType) {
		case ENTITY_CIRCLE: {
			b2CircleShape Shape;
			fDef.shape = &Shape;
			// Set the shape
			Shape.m_radius = screenEntity->getWidth()/(worldScale*2.0f);
			// Create the fixture
			fixture = body->CreateFixture(&fDef);
			break;
		}
		case ENTITY_RECT: {
			b2PolygonShape Shape;
			fDef.shape = &Shape;
			// Set the shape
			Shape.SetAsBox(screenEntity->getWidth()/(worldScale*2.0f) * entityScale.x, screenEntity->getHeight()/(worldScale*2.0f) * entityScale.y);
			// Create the fixture
			fixture = body->CreateFixture(&fDef);
			break;
		}
		case ENTITY_EDGE: {
			b2PolygonShape Shape;
			Shape.SetAsEdge(b2Vec2(-screenEntity->getWidth()/(worldScale*2.0f),-screenEntity->getHeight()/(2.0*worldScale)),
							b2Vec2(screenEntity->getWidth()/(worldScale*2.0f),-screenEntity->getHeight()/(2.0*worldScale)));
			fDef.shape = &Shape;
			fixture = body->CreateFixture(&fDef);
			break;
        }
        break;
		case ENTITY_MESH: {
			b2PolygonShape Shape;
			fDef.shape = &Shape;
			ScreenMesh* screenMesh = dynamic_cast<ScreenMesh*>(entity);

			if(screenMesh) {
				for(short i=0, polycount=screenMesh->getMesh()->getPolygonCount(); i < polycount; i++) {
					Polygon* poly = screenMesh->getMesh()->getPolygon(i);
					unsigned short vertexcount = poly->getVertexCount();
					if (vertexcount >= 3 && vertexcount <= 8) {
						b2Vec2* vertices = new b2Vec2[vertexcount];
						for(short index=0; index < vertexcount; index++) {
							vertices[index].x = poly->getVertex(index)->x/worldScale;
							vertices[index].y = poly->getVertex(index)->y/worldScale;
						}
						// Set the shape
						Shape.Set(vertices, vertexcount);
						// Create the fixture
						fixture = body->CreateFixture(&fDef);
						delete []vertices;
					}
					else { Logger::log("Between 3 and 8 vertices allowed per polygon\n"); }
				}
			}
			else { Logger::log("Tried to make a mesh collision object from a non-mesh\n"); }
		}
	}
}

void PhysicsScreenEntity::applyTorque(Number torque) {
	body->ApplyTorque(torque);
}

void PhysicsScreenEntity::applyForce(Vector2 force){
	body->SetAwake(true);
	body->ApplyForce(b2Vec2(force.x,force.y), b2Vec2(body->GetPosition().x,body->GetPosition().y));
}

ScreenEntity *PhysicsScreenEntity::getScreenEntity() {
	return screenEntity;
}

void PhysicsScreenEntity::setVelocity(Number fx, Number fy) {
	body->SetAwake(true);
	b2Vec2 f = body->GetLinearVelocity();
	if(fx != 0)
		f.x = fx;
	if(fy != 0)
		f.y = fy;
	body->SetLinearVelocity(f);
}

void PhysicsScreenEntity::setVelocityX( Number fx) {
	body->SetAwake(true);
	b2Vec2 f = body->GetLinearVelocity();
	f.x = fx;
	body->SetLinearVelocity(f);
}

void PhysicsScreenEntity::setVelocityY(Number fy) {
	body->SetAwake(true);
	b2Vec2 f = body->GetLinearVelocity();
	f.y = fy;
	body->SetLinearVelocity(f);
}

void PhysicsScreenEntity::setCollisionCategory(int categoryBits) {
        b2Filter filter=fixture->GetFilterData();
        filter.categoryBits = categoryBits;
        fixture->SetFilterData(filter);
}

void PhysicsScreenEntity::setCollisionMask(int maskBits) {
        b2Filter filter=fixture->GetFilterData();
        filter.maskBits = maskBits;
        fixture->SetFilterData(filter);
}

void PhysicsScreenEntity::setCollisionGroupIndex(int group) {
    b2Filter filter=fixture->GetFilterData();
    filter.groupIndex = group;
    fixture->SetFilterData(filter);
}

void PhysicsScreenEntity::setLinearDamping(Number damping) {
    body->SetLinearDamping(damping);
}

void PhysicsScreenEntity::setAngularDamping(Number damping) {
    body->SetAngularDamping(damping);
}

void PhysicsScreenEntity::setFriction(Number friction) {
    if(fixture) {
        fixture->SetFriction(friction);
    }
}

void PhysicsScreenEntity::setDensity(Number density) {
    if(fixture) {
        fixture->SetDensity(density);
    }
}

Number PhysicsScreenEntity::getLinearDamping() {
    return body->GetLinearDamping();
}

Number PhysicsScreenEntity::getAngularDamping() {
    return body->GetAngularDamping();
}

Number PhysicsScreenEntity::getFriction() {
    return fixture->GetFriction();
}

Number PhysicsScreenEntity::getDensity() {
    return fixture->GetDensity();
}

void PhysicsScreenEntity::applyImpulse(Number fx, Number fy) {
	body->SetAwake(true);
	b2Vec2 f =  b2Vec2(fx,fy);
	b2Vec2 p = body->GetWorldPoint(b2Vec2(0.0f, 0.0f));
	body->ApplyLinearImpulse(f, p);
}

void PhysicsScreenEntity::setTransform(Vector2 pos, Number angle) {
	body->SetTransform(b2Vec2(pos.x/worldScale, pos.y/worldScale), angle*(PI/180.0f));
    screenEntity->setPosition(pos);
}

void PhysicsScreenEntity::Update() {
	if(collisionOnly) {
		Matrix4 matrix = screenEntity->getConcatenatedMatrix();
		b2Vec2 newPos;
		Number newRotation;

		Vector3 pos = matrix.getPosition();
		newPos.x = pos.x/worldScale;
		newPos.y = pos.y/worldScale;

		Number rx,ry,rz;

		matrix.getEulerAngles(&rx, &ry, &rz);
		newRotation = rz;

		body->SetAwake(true);
		body->SetTransform(newPos, newRotation * TORADIANS);
	} else {
		b2Vec2 position = body->GetPosition();
		Number angle = body->GetAngle();
		screenEntity->setRotation(angle*(180.0f/PI));
		screenEntity->setPosition(position.x*worldScale, position.y*worldScale);
		screenEntity->rebuildTransformMatrix();
	}
}

b2Fixture* PhysicsScreenEntity::getFixture(unsigned short index) {
	if(fixture)	{
		short i = 0;
		for (b2Fixture* f = body->GetFixtureList(); f; f = f->GetNext()) {
			if (i = index) {
				fixture = f;
				return fixture;
			}
			else {i++;}
		}

		Logger::log("That fixture index does not exist\n");
		return fixture = NULL;
	}

	Logger::log("Fixturelist is for mesh only\n");
	return fixture = NULL;
}

b2Fixture* PhysicsScreenEntity::getFixture() { return fixture; }

// I believe that at runtime you are not supposed to edit Shapes; However you still can
// by getting a fixture(above) and then adding "->GetShape()" on the end to get the fixtures shape

PhysicsScreenEntity::~PhysicsScreenEntity() {
	if(body)
		body->GetWorld()->DestroyBody(body);	// DestroyBody deletes fixtures and shapes automaticaly according to box2d documentation
}<commit_msg>Fixed collision only entity not setting rotation correctly<commit_after>/*
Copyright (C) 2011 by Ivan Safrin

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#define PI 3.14159265

#include "PolyPhysicsScreenEntity.h"
#include "PolyLogger.h"
#include "PolyMesh.h"
#include "PolyPolygon.h"
#include "PolyScreenEntity.h"
#include "PolyScreenMesh.h"

using namespace Polycode;

PhysicsScreenEntity::PhysicsScreenEntity(ScreenEntity *entity, b2World *world, Number worldScale, int entType, bool isStatic, Number friction, Number density, Number restitution, bool isSensor, bool fixedRotation, int groupIndex) {

	screenEntity = entity;

	Vector3 entityScale = entity->getCompoundScale();
	Matrix4 compoundMatrix = screenEntity->getConcatenatedMatrix();
	entity->ignoreParentMatrix = true;
	entity->scale = entityScale;
	this->worldScale = worldScale;
	collisionOnly = false;

	// Create body definition---------------------------------------
	b2BodyDef bodyDef;
	bodyDef.position.Set(compoundMatrix.getPosition().x/worldScale, compoundMatrix.getPosition().y/worldScale);
	bodyDef.angle = screenEntity->getRotation()*(PI/180.0f);
	bodyDef.bullet = isSensor;
	bodyDef.fixedRotation = fixedRotation;
	if(isStatic)
		bodyDef.type = b2_staticBody;
	else
		bodyDef.type = b2_dynamicBody;

	// Create the body
	body = world->CreateBody(&bodyDef);
	body->SetUserData(this);

	// Create fixture definition---------------------------------------------
	b2FixtureDef fDef;
	fDef.friction = friction;
	fDef.restitution = restitution;
	fDef.density = density;
	fDef.isSensor = isSensor;
	fDef.filter.groupIndex = groupIndex;

	// Create Shape definition (Circle/Rectangle/Polygon)---------------------------
	switch(entType) {
		case ENTITY_CIRCLE: {
			b2CircleShape Shape;
			fDef.shape = &Shape;
			// Set the shape
			Shape.m_radius = screenEntity->getWidth()/(worldScale*2.0f);
			// Create the fixture
			fixture = body->CreateFixture(&fDef);
			break;
		}
		case ENTITY_RECT: {
			b2PolygonShape Shape;
			fDef.shape = &Shape;
			// Set the shape
			Shape.SetAsBox(screenEntity->getWidth()/(worldScale*2.0f) * entityScale.x, screenEntity->getHeight()/(worldScale*2.0f) * entityScale.y);
			// Create the fixture
			fixture = body->CreateFixture(&fDef);
			break;
		}
		case ENTITY_EDGE: {
			b2PolygonShape Shape;
			Shape.SetAsEdge(b2Vec2(-screenEntity->getWidth()/(worldScale*2.0f),-screenEntity->getHeight()/(2.0*worldScale)),
							b2Vec2(screenEntity->getWidth()/(worldScale*2.0f),-screenEntity->getHeight()/(2.0*worldScale)));
			fDef.shape = &Shape;
			fixture = body->CreateFixture(&fDef);
			break;
        }
        break;
		case ENTITY_MESH: {
			b2PolygonShape Shape;
			fDef.shape = &Shape;
			ScreenMesh* screenMesh = dynamic_cast<ScreenMesh*>(entity);

			if(screenMesh) {
				for(short i=0, polycount=screenMesh->getMesh()->getPolygonCount(); i < polycount; i++) {
					Polygon* poly = screenMesh->getMesh()->getPolygon(i);
					unsigned short vertexcount = poly->getVertexCount();
					if (vertexcount >= 3 && vertexcount <= 8) {
						b2Vec2* vertices = new b2Vec2[vertexcount];
						for(short index=0; index < vertexcount; index++) {
							vertices[index].x = poly->getVertex(index)->x/worldScale;
							vertices[index].y = poly->getVertex(index)->y/worldScale;
						}
						// Set the shape
						Shape.Set(vertices, vertexcount);
						// Create the fixture
						fixture = body->CreateFixture(&fDef);
						delete []vertices;
					}
					else { Logger::log("Between 3 and 8 vertices allowed per polygon\n"); }
				}
			}
			else { Logger::log("Tried to make a mesh collision object from a non-mesh\n"); }
		}
	}
}

void PhysicsScreenEntity::applyTorque(Number torque) {
	body->ApplyTorque(torque);
}

void PhysicsScreenEntity::applyForce(Vector2 force){
	body->SetAwake(true);
	body->ApplyForce(b2Vec2(force.x,force.y), b2Vec2(body->GetPosition().x,body->GetPosition().y));
}

ScreenEntity *PhysicsScreenEntity::getScreenEntity() {
	return screenEntity;
}

void PhysicsScreenEntity::setVelocity(Number fx, Number fy) {
	body->SetAwake(true);
	b2Vec2 f = body->GetLinearVelocity();
	if(fx != 0)
		f.x = fx;
	if(fy != 0)
		f.y = fy;
	body->SetLinearVelocity(f);
}

void PhysicsScreenEntity::setVelocityX( Number fx) {
	body->SetAwake(true);
	b2Vec2 f = body->GetLinearVelocity();
	f.x = fx;
	body->SetLinearVelocity(f);
}

void PhysicsScreenEntity::setVelocityY(Number fy) {
	body->SetAwake(true);
	b2Vec2 f = body->GetLinearVelocity();
	f.y = fy;
	body->SetLinearVelocity(f);
}

void PhysicsScreenEntity::setCollisionCategory(int categoryBits) {
        b2Filter filter=fixture->GetFilterData();
        filter.categoryBits = categoryBits;
        fixture->SetFilterData(filter);
}

void PhysicsScreenEntity::setCollisionMask(int maskBits) {
        b2Filter filter=fixture->GetFilterData();
        filter.maskBits = maskBits;
        fixture->SetFilterData(filter);
}

void PhysicsScreenEntity::setCollisionGroupIndex(int group) {
    b2Filter filter=fixture->GetFilterData();
    filter.groupIndex = group;
    fixture->SetFilterData(filter);
}

void PhysicsScreenEntity::setLinearDamping(Number damping) {
    body->SetLinearDamping(damping);
}

void PhysicsScreenEntity::setAngularDamping(Number damping) {
    body->SetAngularDamping(damping);
}

void PhysicsScreenEntity::setFriction(Number friction) {
    if(fixture) {
        fixture->SetFriction(friction);
    }
}

void PhysicsScreenEntity::setDensity(Number density) {
    if(fixture) {
        fixture->SetDensity(density);
    }
}

Number PhysicsScreenEntity::getLinearDamping() {
    return body->GetLinearDamping();
}

Number PhysicsScreenEntity::getAngularDamping() {
    return body->GetAngularDamping();
}

Number PhysicsScreenEntity::getFriction() {
    return fixture->GetFriction();
}

Number PhysicsScreenEntity::getDensity() {
    return fixture->GetDensity();
}

void PhysicsScreenEntity::applyImpulse(Number fx, Number fy) {
	body->SetAwake(true);
	b2Vec2 f =  b2Vec2(fx,fy);
	b2Vec2 p = body->GetWorldPoint(b2Vec2(0.0f, 0.0f));
	body->ApplyLinearImpulse(f, p);
}

void PhysicsScreenEntity::setTransform(Vector2 pos, Number angle) {
	body->SetTransform(b2Vec2(pos.x/worldScale, pos.y/worldScale), angle*(PI/180.0f));
    screenEntity->setPosition(pos);
}

void PhysicsScreenEntity::Update() {
	if(collisionOnly) {
		b2Vec2 newPos;
		newPos.x = screenEntity->position.x/worldScale;;
		newPos.y = screenEntity->position.y/worldScale;;
		body->SetAwake(true);
		body->SetTransform(newPos, screenEntity->rotation.roll * TORADIANS);
	} else {
		b2Vec2 position = body->GetPosition();
		Number angle = body->GetAngle();
		screenEntity->setRotation(angle*(180.0f/PI));
		screenEntity->setPosition(position.x*worldScale, position.y*worldScale);
		screenEntity->rebuildTransformMatrix();
	}
}

b2Fixture* PhysicsScreenEntity::getFixture(unsigned short index) {
	if(fixture)	{
		short i = 0;
		for (b2Fixture* f = body->GetFixtureList(); f; f = f->GetNext()) {
			if (i = index) {
				fixture = f;
				return fixture;
			}
			else {i++;}
		}

		Logger::log("That fixture index does not exist\n");
		return fixture = NULL;
	}

	Logger::log("Fixturelist is for mesh only\n");
	return fixture = NULL;
}

b2Fixture* PhysicsScreenEntity::getFixture() { return fixture; }

// I believe that at runtime you are not supposed to edit Shapes; However you still can
// by getting a fixture(above) and then adding "->GetShape()" on the end to get the fixtures shape

PhysicsScreenEntity::~PhysicsScreenEntity() {
	if(body)
		body->GetWorld()->DestroyBody(body);	// DestroyBody deletes fixtures and shapes automaticaly according to box2d documentation
}<|endoftext|>


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

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date: 2008-02-25 17:27:17 +0100 (Mo, 25 Feb 2008) $
Version:   $Revision: 7837 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

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

//Poco headers
#include "Poco/Path.h"

//mitk headers
#include "mitkNavigationToolWriter.h"
#include "mitkCommon.h"
#include "mitkTestingMacros.h"
#include "mitkStandardFileLocations.h"
#include "mitkNavigationTool.h"
#include "mitkSTLFileReader.h"
#include "mitkBaseData.h"
#include "mitkDataTreeNode.h"
#include "mitkSurface.h"
#include "mitkStandaloneDataStorage.h"
#include "mitkDataStorage.h"
#include "mitkNavigationToolReader.h"

#include <sstream>

class mitkNavigationToolReaderAndWriterTestClass
  {
  private:

    static mitk::Surface::Pointer testSurface;

  public:

    static void TestInstantiation()
    {
    // let's create an object of our class
    mitk::NavigationToolWriter::Pointer myWriter = mitk::NavigationToolWriter::New();
    MITK_TEST_CONDITION_REQUIRED(myWriter.IsNotNull(),"Testing instantiation")
    }

    static void TestWrite()
    {
    //create a NavigationTool which we can write on the harddisc
    std::string toolFileName = mitk::StandardFileLocations::GetInstance()->FindFile("ClaronTool", "Modules/IGT/Testing/Data");
    MITK_TEST_CONDITION(toolFileName.empty() == false, "Check if tool calibration file exists");
    mitk::NavigationTool::Pointer myNavigationTool = mitk::NavigationTool::New();
    myNavigationTool->SetCalibrationFile(toolFileName);

    mitk::DataTreeNode::Pointer myNode = mitk::DataTreeNode::New();
    myNode->SetName("ClaronTool");

      //load an stl File
      mitk::STLFileReader::Pointer stlReader = mitk::STLFileReader::New();
      try
        {
        stlReader->SetFileName( mitk::StandardFileLocations::GetInstance()->FindFile("ClaronTool.stl", "Testing/Data/").c_str() );
        stlReader->Update();
        }
      catch (...)
        {
        MITK_TEST_FAILED_MSG(<<"Cannot read stl file.");
        }

      if ( stlReader->GetOutput() == NULL )
        {
        MITK_TEST_FAILED_MSG(<<"Cannot read stl file.");
        }
      else
        {
        testSurface = stlReader->GetOutput();
        myNode->SetData(testSurface);
        }

    myNavigationTool->SetDataTreeNode(myNode);
    myNavigationTool->SetIdentifier("ClaronTool#1");
    myNavigationTool->SetSerialNumber("0815");
    myNavigationTool->SetTrackingDeviceType(mitk::ClaronMicron);
    myNavigationTool->SetType(mitk::NavigationTool::Fiducial);

    //now create a writer and write it to the harddisc
    mitk::NavigationToolWriter::Pointer myWriter = mitk::NavigationToolWriter::New();
    std::string filename = mitk::StandardFileLocations::GetInstance()->GetOptionDirectory()+Poco::Path::separator()+".."+Poco::Path::separator()+"TestTool.tool";

    MITK_TEST_OUTPUT(<<"---- Testing navigation tool writer ----");
    bool test = myWriter->DoWrite(filename,myNavigationTool);
    MITK_TEST_CONDITION_REQUIRED(test,"OK");
    }

    static void TestRead()
    {
    mitk::DataStorage::Pointer testStorage = mitk::StandaloneDataStorage::New();
    mitk::NavigationToolReader::Pointer myReader = mitk::NavigationToolReader::New(testStorage);
    mitk::NavigationTool::Pointer readTool = myReader->DoRead(mitk::StandardFileLocations::GetInstance()->GetOptionDirectory()+Poco::Path::separator()+".."+Poco::Path::separator()+"TestTool.tool");
    MITK_TEST_OUTPUT(<<"---- Testing navigation tool reader ----");
    MITK_TEST_CONDITION_REQUIRED(readTool->GetDataTreeNode() == testStorage->GetNamedNode(readTool->GetDataTreeNode()->GetName()),"Test if tool was added to storage...");
    MITK_TEST_CONDITION_REQUIRED(readTool->GetDataTreeNode()->GetData()==testSurface,"Test if surface was restored correctly ...");
    //MITK_TEST_CONDITION_REQUIRED();
    }

    static void CleanUp()
    {
    std::remove((mitk::StandardFileLocations::GetInstance()->GetOptionDirectory()+Poco::Path::separator()+".."+Poco::Path::separator()+"TestTool.tool").c_str());
    }

  };

mitk::Surface::Pointer mitkNavigationToolReaderAndWriterTestClass::testSurface = NULL;

/** This function is testing the TrackingVolume class. */
int mitkNavigationToolReaderAndWriterTest(int /* argc */, char* /*argv*/[])
{
  MITK_TEST_BEGIN("NavigationToolWriter")

  mitkNavigationToolReaderAndWriterTestClass::TestInstantiation();
  mitkNavigationToolReaderAndWriterTestClass::TestWrite();
  //mitkNavigationToolReaderAndWriterTestClass::TestRead();
  mitkNavigationToolReaderAndWriterTestClass::CleanUp();

  MITK_TEST_END()
}

<commit_msg>COMP: commented out some non-compiling code for while.<commit_after>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Language:  C++
Date:      $Date: 2008-02-25 17:27:17 +0100 (Mo, 25 Feb 2008) $
Version:   $Revision: 7837 $

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

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

//Poco headers
#include "Poco/Path.h"

//mitk headers
#include "mitkNavigationToolWriter.h"
#include "mitkCommon.h"
#include "mitkTestingMacros.h"
#include "mitkStandardFileLocations.h"
#include "mitkNavigationTool.h"
#include "mitkSTLFileReader.h"
#include "mitkBaseData.h"
#include "mitkDataTreeNode.h"
#include "mitkSurface.h"
#include "mitkStandaloneDataStorage.h"
#include "mitkDataStorage.h"
#include "mitkNavigationToolReader.h"

#include <sstream>

class mitkNavigationToolReaderAndWriterTestClass
  {
  private:

    static mitk::Surface::Pointer testSurface;

  public:

    static void TestInstantiation()
    {
    // let's create an object of our class
    mitk::NavigationToolWriter::Pointer myWriter = mitk::NavigationToolWriter::New();
    MITK_TEST_CONDITION_REQUIRED(myWriter.IsNotNull(),"Testing instantiation")
    }

    static void TestWrite()
    {
    //create a NavigationTool which we can write on the harddisc
    std::string toolFileName = mitk::StandardFileLocations::GetInstance()->FindFile("ClaronTool", "Modules/IGT/Testing/Data");
    MITK_TEST_CONDITION(toolFileName.empty() == false, "Check if tool calibration file exists");
    mitk::NavigationTool::Pointer myNavigationTool = mitk::NavigationTool::New();
    myNavigationTool->SetCalibrationFile(toolFileName);

    mitk::DataTreeNode::Pointer myNode = mitk::DataTreeNode::New();
    myNode->SetName("ClaronTool");

      //load an stl File
      mitk::STLFileReader::Pointer stlReader = mitk::STLFileReader::New();
      try
        {
        stlReader->SetFileName( mitk::StandardFileLocations::GetInstance()->FindFile("ClaronTool.stl", "Testing/Data/").c_str() );
        stlReader->Update();
        }
      catch (...)
        {
        MITK_TEST_FAILED_MSG(<<"Cannot read stl file.");
        }

      if ( stlReader->GetOutput() == NULL )
        {
        MITK_TEST_FAILED_MSG(<<"Cannot read stl file.");
        }
      else
        {
        testSurface = stlReader->GetOutput();
        myNode->SetData(testSurface);
        }

    myNavigationTool->SetDataTreeNode(myNode);
    myNavigationTool->SetIdentifier("ClaronTool#1");
    myNavigationTool->SetSerialNumber("0815");
    myNavigationTool->SetTrackingDeviceType(mitk::ClaronMicron);
    myNavigationTool->SetType(mitk::NavigationTool::Fiducial);

    //now create a writer and write it to the harddisc
    mitk::NavigationToolWriter::Pointer myWriter = mitk::NavigationToolWriter::New();
    std::string filename = mitk::StandardFileLocations::GetInstance()->GetOptionDirectory()+Poco::Path::separator()+".."+Poco::Path::separator()+"TestTool.tool";

    MITK_TEST_OUTPUT(<<"---- Testing navigation tool writer ----");
    bool test = myWriter->DoWrite(filename,myNavigationTool);
    MITK_TEST_CONDITION_REQUIRED(test,"OK");
    }

    static void TestRead()
    {
    /*
    mitk::DataStorage::Pointer testStorage = mitk::StandaloneDataStorage::New(); TODO: DIESE STELLE UNTER LINUX ZUM LAUFEN BRINGEN
    mitk::NavigationToolReader::Pointer myReader = mitk::NavigationToolReader::New(testStorage);
    mitk::NavigationTool::Pointer readTool = myReader->DoRead(mitk::StandardFileLocations::GetInstance()->GetOptionDirectory()+Poco::Path::separator()+".."+Poco::Path::separator()+"TestTool.tool");
    MITK_TEST_OUTPUT(<<"---- Testing navigation tool reader ----");
    MITK_TEST_CONDITION_REQUIRED(readTool->GetDataTreeNode() == testStorage->GetNamedNode(readTool->GetDataTreeNode()->GetName()),"Test if tool was added to storage...");
    MITK_TEST_CONDITION_REQUIRED(readTool->GetDataTreeNode()->GetData()==testSurface,"Test if surface was restored correctly ...");
    */
    //MITK_TEST_CONDITION_REQUIRED();
    }

    static void CleanUp()
    {
    std::remove((mitk::StandardFileLocations::GetInstance()->GetOptionDirectory()+Poco::Path::separator()+".."+Poco::Path::separator()+"TestTool.tool").c_str());
    }

  };

mitk::Surface::Pointer mitkNavigationToolReaderAndWriterTestClass::testSurface = NULL;

/** This function is testing the TrackingVolume class. */
int mitkNavigationToolReaderAndWriterTest(int /* argc */, char* /*argv*/[])
{
  MITK_TEST_BEGIN("NavigationToolWriter")

  mitkNavigationToolReaderAndWriterTestClass::TestInstantiation();
  mitkNavigationToolReaderAndWriterTestClass::TestWrite();
  //mitkNavigationToolReaderAndWriterTestClass::TestRead();
  mitkNavigationToolReaderAndWriterTestClass::CleanUp();

  MITK_TEST_END()
}

<|endoftext|>


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

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include <mitkToFDistanceImageToSurfaceFilter.h>
#include <mitkInstantiateAccessFunctions.h>
#include <mitkSurface.h>
#include "mitkImageReadAccessor.h"

#include <itkImage.h>

#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkFloatArray.h>
#include <vtkSmartPointer.h>
#include <vtkIdList.h>

#include <math.h>
#include <vtkMath.h>

mitk::ToFDistanceImageToSurfaceFilter::ToFDistanceImageToSurfaceFilter() :
  m_IplScalarImage(NULL), m_CameraIntrinsics(), m_TextureImageWidth(0), m_TextureImageHeight(0), m_InterPixelDistance(), m_TextureIndex(0),
  m_GenerateTriangularMesh(true), m_TriangulationThreshold(0.0)
{
  m_InterPixelDistance.Fill(0.045);
  m_CameraIntrinsics = mitk::CameraIntrinsics::New();
  m_CameraIntrinsics->SetFocalLength(273.138946533,273.485900879);
  m_CameraIntrinsics->SetPrincipalPoint(107.867935181,98.3807373047);
  m_CameraIntrinsics->SetDistorsionCoeffs(-0.486690014601f,0.553943634033f,0.00222016777843f,-0.00300851115026f);
  m_ReconstructionMode = WithInterPixelDistance;
}

mitk::ToFDistanceImageToSurfaceFilter::~ToFDistanceImageToSurfaceFilter()
{
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput( Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics )
{
  this->SetCameraIntrinsics(cameraIntrinsics);
  this->SetInput(0,distanceImage);
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics )
{
  this->SetCameraIntrinsics(cameraIntrinsics);
  this->SetInput(idx,distanceImage);
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput(  mitk::Image* distanceImage )
{
  this->SetInput(0,distanceImage);
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  mitk::Image* distanceImage )
{
  if ((distanceImage == NULL) && (idx == this->GetNumberOfInputs() - 1)) // if the last input is set to NULL, reduce the number of inputs by one
    this->SetNumberOfInputs(this->GetNumberOfInputs() - 1);
  else
    this->ProcessObject::SetNthInput(idx, distanceImage);   // Process object is not const-correct so the const_cast is required here

  this->CreateOutputsForAllInputs();
}

mitk::Image* mitk::ToFDistanceImageToSurfaceFilter::GetInput()
{
  return this->GetInput(0);
}

mitk::Image* mitk::ToFDistanceImageToSurfaceFilter::GetInput( unsigned int idx )
{
  if (this->GetNumberOfInputs() < 1)
  {
    mitkThrow() << "No input given for ToFDistanceImageToSurfaceFilter";
  }
  return static_cast< mitk::Image*>(this->ProcessObject::GetInput(idx));
}

void mitk::ToFDistanceImageToSurfaceFilter::GenerateData()
{
  mitk::Surface::Pointer output = this->GetOutput();
  assert(output);
  mitk::Image::Pointer input = this->GetInput();
  assert(input);
  // mesh points
  int xDimension = input->GetDimension(0);
  int yDimension = input->GetDimension(1);
  unsigned int size = xDimension*yDimension; //size of the image-array
  std::vector<bool> isPointValid;
  isPointValid.resize(size);
  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
  points->SetDataTypeToDouble();
  vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkFloatArray> scalarArray = vtkSmartPointer<vtkFloatArray>::New();
  vtkSmartPointer<vtkFloatArray> textureCoords = vtkSmartPointer<vtkFloatArray>::New();
  textureCoords->SetNumberOfComponents(2);
  textureCoords->Allocate(size);

  //Make a vtkIdList to save the ID's of the polyData corresponding to the image
  //pixel ID's. See below for more documentation.
  m_VertexIdList = vtkSmartPointer<vtkIdList>::New();
  //Allocate the object once else it would automatically allocate new memory
  //for every vertex and perform a copy which is expensive.
  m_VertexIdList->Allocate(size);
  m_VertexIdList->SetNumberOfIds(size);
  for(unsigned int i = 0; i < size; ++i)
  {
    m_VertexIdList->SetId(i, 0);
  }

  float* scalarFloatData = NULL;

  if (this->m_IplScalarImage) // if scalar image is defined use it for texturing
  {
    scalarFloatData = (float*)this->m_IplScalarImage->imageData;
  }
  else if (this->GetInput(m_TextureIndex)) // otherwise use intensity image (input(2))
  {
    ImageReadAccessor inputAcc(this->GetInput(m_TextureIndex));
    scalarFloatData = (float*)inputAcc.GetData();
  }

  ImageReadAccessor inputAcc(input, input->GetSliceData(0,0,0));
  float* inputFloatData = (float*)inputAcc.GetData();
  //calculate world coordinates
  mitk::ToFProcessingCommon::ToFPoint2D focalLengthInPixelUnits;
  mitk::ToFProcessingCommon::ToFScalarType focalLengthInMm;
  if((m_ReconstructionMode == WithOutInterPixelDistance) || (m_ReconstructionMode == Kinect))
  {
    focalLengthInPixelUnits[0] = m_CameraIntrinsics->GetFocalLengthX();
    focalLengthInPixelUnits[1] = m_CameraIntrinsics->GetFocalLengthY();
  }
  else if( m_ReconstructionMode == WithInterPixelDistance)
  {
    //convert focallength from pixel to mm
    focalLengthInMm = (m_CameraIntrinsics->GetFocalLengthX()*m_InterPixelDistance[0]+m_CameraIntrinsics->GetFocalLengthY()*m_InterPixelDistance[1])/2.0;
  }

  mitk::ToFProcessingCommon::ToFPoint2D principalPoint;
  principalPoint[0] = m_CameraIntrinsics->GetPrincipalPointX();
  principalPoint[1] = m_CameraIntrinsics->GetPrincipalPointY();

  mitk::Point3D origin = input->GetGeometry()->GetOrigin();

  for (int j=0; j<yDimension; j++)
  {
    for (int i=0; i<xDimension; i++)
    {
      unsigned int pixelID = i+j*xDimension;

      mitk::ToFProcessingCommon::ToFScalarType distance = (double)inputFloatData[pixelID];

      mitk::ToFProcessingCommon::ToFPoint3D cartesianCoordinates;
      switch (m_ReconstructionMode)
      {
      case WithOutInterPixelDistance:
      {
        cartesianCoordinates = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i+origin[0],j+origin[1],distance,focalLengthInPixelUnits,principalPoint);
        break;
      }
      case WithInterPixelDistance:
      {
        cartesianCoordinates = mitk::ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(i+origin[0],j+origin[1],distance,focalLengthInMm,m_InterPixelDistance,principalPoint);
        break;
      }
      case Kinect:
      {
        cartesianCoordinates = mitk::ToFProcessingCommon::KinectIndexToCartesianCoordinates(i+origin[0],j+origin[1],distance,focalLengthInPixelUnits,principalPoint);
        break;
      }
      default:
      {
        MITK_ERROR << "Incorrect reconstruction mode!";
      }
      }
      //Epsilon here, because we may have small float values like 0.00000001 which in fact represents 0.
      if (distance<=mitk::eps)
      {
        isPointValid[pixelID] = false;
      }
      else
      {
        isPointValid[pixelID] = true;
        //VTK would insert empty points into the polydata if we use
        //points->InsertPoint(pixelID, cartesianCoordinates.GetDataPointer()).
        //If we use points->InsertNextPoint(...) instead, the ID's do not
        //correspond to the image pixel ID's. Thus, we have to save them
        //in the vertexIdList.
        m_VertexIdList->SetId(pixelID, points->InsertNextPoint(cartesianCoordinates.GetDataPointer()));

        if (m_GenerateTriangularMesh)
        {
          if((i >= 1) && (j >= 1))
          {
            //This little piece of art explains the ID's:
            //
            // P(x_1y_1)---P(xy_1)
            // |           |
            // |           |
            // |           |
            // P(x_1y)-----P(xy)
            //
            //We can only start triangulation if we are at vertex (1,1),
            //because we need the other 3 vertices near this one.
            //To go one pixel line back in the image array, we have to
            //subtract 1x xDimension.
            vtkIdType xy = pixelID;
            vtkIdType x_1y = pixelID-1;
            vtkIdType xy_1 = pixelID-xDimension;
            vtkIdType x_1y_1 = xy_1-1;

            //Find the corresponding vertex ID's in the saved vertexIdList:
            vtkIdType xyV = m_VertexIdList->GetId(xy);
            vtkIdType x_1yV = m_VertexIdList->GetId(x_1y);
            vtkIdType xy_1V = m_VertexIdList->GetId(xy_1);
            vtkIdType x_1y_1V = m_VertexIdList->GetId(x_1y_1);

            if (isPointValid[xy]&&isPointValid[x_1y]&&isPointValid[x_1y_1]&&isPointValid[xy_1]) // check if points of cell are valid
            {
              double pointXY[3], pointX_1Y[3], pointXY_1[3], pointX_1Y_1[3];

              points->GetPoint(xyV, pointXY);
              points->GetPoint(x_1yV, pointX_1Y);
              points->GetPoint(xy_1V, pointXY_1);
              points->GetPoint(x_1y_1V, pointX_1Y_1);

              if( (mitk::Equal(m_TriangulationThreshold, 0.0)) || ((vtkMath::Distance2BetweenPoints(pointXY, pointX_1Y) <= m_TriangulationThreshold)
                                                                   && (vtkMath::Distance2BetweenPoints(pointXY, pointXY_1) <= m_TriangulationThreshold)
                                                                   && (vtkMath::Distance2BetweenPoints(pointX_1Y, pointX_1Y_1) <= m_TriangulationThreshold)
                                                                   && (vtkMath::Distance2BetweenPoints(pointXY_1, pointX_1Y_1) <= m_TriangulationThreshold)))
              {
                polys->InsertNextCell(3);
                polys->InsertCellPoint(x_1yV);
                polys->InsertCellPoint(xyV);
                polys->InsertCellPoint(x_1y_1V);

                polys->InsertNextCell(3);
                polys->InsertCellPoint(x_1y_1V);
                polys->InsertCellPoint(xyV);
                polys->InsertCellPoint(xy_1V);
              }
              else
              {
                //We dont want triangulation, but we want to keep the vertex
                vertices->InsertNextCell(1);
                vertices->InsertCellPoint(xyV);
              }
            }

            //Scalar values are necessary for mapping colors/texture onto the surface
            if (scalarFloatData)
            {
              scalarArray->InsertTuple1(m_VertexIdList->GetId(pixelID), scalarFloatData[pixelID]);
            }
            //These Texture Coordinates will map color pixel and vertices 1:1 (e.g. for Kinect).
            float xNorm = (((float)i)/xDimension);// correct video texture scale for kinect
            float yNorm = ((float)j)/yDimension; //don't flip. we don't need to flip.
            textureCoords->InsertTuple2(m_VertexIdList->GetId(pixelID), xNorm, yNorm);
          }
        }
        else
        {
          //We dont want triangulation, we only want vertices
          vertices->InsertNextCell(1);
          vertices->InsertCellPoint(m_VertexIdList->GetId(pixelID));
        }
      }
    }
  }

  vtkSmartPointer<vtkPolyData> mesh = vtkSmartPointer<vtkPolyData>::New();
  mesh->SetPoints(points);
  mesh->SetPolys(polys);
  mesh->SetVerts(vertices);
  //Pass the scalars to the polydata (if they were set).
  if (scalarArray->GetNumberOfTuples()>0)
  {
    mesh->GetPointData()->SetScalars(scalarArray);
  }
  //Pass the TextureCoords to the polydata anyway (to save them).
  mesh->GetPointData()->SetTCoords(textureCoords);
  output->SetVtkPolyData(mesh);
}

void mitk::ToFDistanceImageToSurfaceFilter::CreateOutputsForAllInputs()
{
  this->SetNumberOfOutputs(this->GetNumberOfInputs());  // create outputs for all inputs
  for (unsigned int idx = 0; idx < this->GetNumberOfOutputs(); ++idx)
    if (this->GetOutput(idx) == NULL)
    {
      DataObjectPointer newOutput = this->MakeOutput(idx);
      this->SetNthOutput(idx, newOutput);
    }
  this->Modified();
}

void mitk::ToFDistanceImageToSurfaceFilter::GenerateOutputInformation()
{
  this->GetOutput();
  itkDebugMacro(<<"GenerateOutputInformation()");

}

void mitk::ToFDistanceImageToSurfaceFilter::SetScalarImage(IplImage* iplScalarImage)
{
  this->m_IplScalarImage = iplScalarImage;
  this->Modified();
}

IplImage* mitk::ToFDistanceImageToSurfaceFilter::GetScalarImage()
{
  return this->m_IplScalarImage;
}

void mitk::ToFDistanceImageToSurfaceFilter::SetTextureImageWidth(int width)
{
  this->m_TextureImageWidth = width;
}

void mitk::ToFDistanceImageToSurfaceFilter::SetTextureImageHeight(int height)
{
  this->m_TextureImageHeight = height;
}

void mitk::ToFDistanceImageToSurfaceFilter::SetTriangulationThreshold(double triangulationThreshold)
{
  //vtkMath::Distance2BetweenPoints returns the squared distance between two points and
  //hence we square m_TriangulationThreshold in order to save run-time.
  this->m_TriangulationThreshold = triangulationThreshold*triangulationThreshold;
}
<commit_msg>In TofDistanceImageToSurfaceFilter float* were used which created subtle errors since they pointed to double values.<commit_after>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

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

#include <mitkToFDistanceImageToSurfaceFilter.h>
#include <mitkInstantiateAccessFunctions.h>
#include <mitkSurface.h>
#include "mitkImageReadAccessor.h"

#include <itkImage.h>

#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPointData.h>
#include <vtkFloatArray.h>
#include <vtkSmartPointer.h>
#include <vtkIdList.h>

#include <math.h>
#include <vtkMath.h>

mitk::ToFDistanceImageToSurfaceFilter::ToFDistanceImageToSurfaceFilter() :
  m_IplScalarImage(NULL), m_CameraIntrinsics(), m_TextureImageWidth(0), m_TextureImageHeight(0), m_InterPixelDistance(), m_TextureIndex(0),
  m_GenerateTriangularMesh(true), m_TriangulationThreshold(0.0)
{
  m_InterPixelDistance.Fill(0.045);
  m_CameraIntrinsics = mitk::CameraIntrinsics::New();
  m_CameraIntrinsics->SetFocalLength(273.138946533,273.485900879);
  m_CameraIntrinsics->SetPrincipalPoint(107.867935181,98.3807373047);
  m_CameraIntrinsics->SetDistorsionCoeffs(-0.486690014601f,0.553943634033f,0.00222016777843f,-0.00300851115026f);
  m_ReconstructionMode = WithInterPixelDistance;
}

mitk::ToFDistanceImageToSurfaceFilter::~ToFDistanceImageToSurfaceFilter()
{
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput( Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics )
{
  this->SetCameraIntrinsics(cameraIntrinsics);
  this->SetInput(0,distanceImage);
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  Image* distanceImage, mitk::CameraIntrinsics::Pointer cameraIntrinsics )
{
  this->SetCameraIntrinsics(cameraIntrinsics);
  this->SetInput(idx,distanceImage);
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput(  mitk::Image* distanceImage )
{
  this->SetInput(0,distanceImage);
}

void mitk::ToFDistanceImageToSurfaceFilter::SetInput( unsigned int idx,  mitk::Image* distanceImage )
{
  if ((distanceImage == NULL) && (idx == this->GetNumberOfInputs() - 1)) // if the last input is set to NULL, reduce the number of inputs by one
    this->SetNumberOfInputs(this->GetNumberOfInputs() - 1);
  else
    this->ProcessObject::SetNthInput(idx, distanceImage);   // Process object is not const-correct so the const_cast is required here

  this->CreateOutputsForAllInputs();
}

mitk::Image* mitk::ToFDistanceImageToSurfaceFilter::GetInput()
{
  return this->GetInput(0);
}

mitk::Image* mitk::ToFDistanceImageToSurfaceFilter::GetInput( unsigned int idx )
{
  if (this->GetNumberOfInputs() < 1)
  {
    mitkThrow() << "No input given for ToFDistanceImageToSurfaceFilter";
  }
  return static_cast< mitk::Image*>(this->ProcessObject::GetInput(idx));
}

void mitk::ToFDistanceImageToSurfaceFilter::GenerateData()
{
  mitk::Surface::Pointer output = this->GetOutput();
  assert(output);
  mitk::Image::Pointer input = this->GetInput();
  assert(input);
  // mesh points
  int xDimension = input->GetDimension(0);
  int yDimension = input->GetDimension(1);
  unsigned int size = xDimension*yDimension; //size of the image-array
  std::vector<bool> isPointValid;
  isPointValid.resize(size);
  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
  points->SetDataTypeToDouble();
  vtkSmartPointer<vtkCellArray> polys = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkCellArray> vertices = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkFloatArray> scalarArray = vtkSmartPointer<vtkFloatArray>::New();
  vtkSmartPointer<vtkFloatArray> textureCoords = vtkSmartPointer<vtkFloatArray>::New();
  textureCoords->SetNumberOfComponents(2);
  textureCoords->Allocate(size);

  //Make a vtkIdList to save the ID's of the polyData corresponding to the image
  //pixel ID's. See below for more documentation.
  m_VertexIdList = vtkSmartPointer<vtkIdList>::New();
  //Allocate the object once else it would automatically allocate new memory
  //for every vertex and perform a copy which is expensive.
  m_VertexIdList->Allocate(size);
  m_VertexIdList->SetNumberOfIds(size);
  for(unsigned int i = 0; i < size; ++i)
  {
    m_VertexIdList->SetId(i, 0);
  }

  ScalarType* scalarFloatData = NULL;

  if (this->m_IplScalarImage) // if scalar image is defined use it for texturing
  {
    scalarFloatData = (ScalarType*)this->m_IplScalarImage->imageData;
  }
  else if (this->GetInput(m_TextureIndex)) // otherwise use intensity image (input(2))
  {
    ImageReadAccessor inputAcc(this->GetInput(m_TextureIndex));
    scalarFloatData = (ScalarType*)inputAcc.GetData();
  }

  ImageReadAccessor inputAcc(input, input->GetSliceData(0,0,0));
  ScalarType* inputFloatData = (ScalarType*)inputAcc.GetData();
  //calculate world coordinates
  mitk::ToFProcessingCommon::ToFPoint2D focalLengthInPixelUnits;
  mitk::ToFProcessingCommon::ToFScalarType focalLengthInMm;
  if((m_ReconstructionMode == WithOutInterPixelDistance) || (m_ReconstructionMode == Kinect))
  {
    focalLengthInPixelUnits[0] = m_CameraIntrinsics->GetFocalLengthX();
    focalLengthInPixelUnits[1] = m_CameraIntrinsics->GetFocalLengthY();
  }
  else if( m_ReconstructionMode == WithInterPixelDistance)
  {
    //convert focallength from pixel to mm
    focalLengthInMm = (m_CameraIntrinsics->GetFocalLengthX()*m_InterPixelDistance[0]+m_CameraIntrinsics->GetFocalLengthY()*m_InterPixelDistance[1])/2.0;
  }

  mitk::ToFProcessingCommon::ToFPoint2D principalPoint;
  principalPoint[0] = m_CameraIntrinsics->GetPrincipalPointX();
  principalPoint[1] = m_CameraIntrinsics->GetPrincipalPointY();

  mitk::Point3D origin = input->GetGeometry()->GetOrigin();

  for (int j=0; j<yDimension; j++)
  {
    for (int i=0; i<xDimension; i++)
    {
      unsigned int pixelID = i+j*xDimension;

      mitk::ToFProcessingCommon::ToFScalarType distance = (double)inputFloatData[pixelID];

      mitk::ToFProcessingCommon::ToFPoint3D cartesianCoordinates;
      switch (m_ReconstructionMode)
      {
      case WithOutInterPixelDistance:
      {
        cartesianCoordinates = mitk::ToFProcessingCommon::IndexToCartesianCoordinates(i+origin[0],j+origin[1],distance,focalLengthInPixelUnits,principalPoint);
        break;
      }
      case WithInterPixelDistance:
      {
        cartesianCoordinates = mitk::ToFProcessingCommon::IndexToCartesianCoordinatesWithInterpixdist(i+origin[0],j+origin[1],distance,focalLengthInMm,m_InterPixelDistance,principalPoint);
        break;
      }
      case Kinect:
      {
        cartesianCoordinates = mitk::ToFProcessingCommon::KinectIndexToCartesianCoordinates(i+origin[0],j+origin[1],distance,focalLengthInPixelUnits,principalPoint);
        break;
      }
      default:
      {
        MITK_ERROR << "Incorrect reconstruction mode!";
      }
      }
      //Epsilon here, because we may have small ScalarType values like 0.00000001 which in fact represents 0.
      if (distance<=mitk::eps)
      {
        isPointValid[pixelID] = false;
      }
      else
      {
        isPointValid[pixelID] = true;
        //VTK would insert empty points into the polydata if we use
        //points->InsertPoint(pixelID, cartesianCoordinates.GetDataPointer()).
        //If we use points->InsertNextPoint(...) instead, the ID's do not
        //correspond to the image pixel ID's. Thus, we have to save them
        //in the vertexIdList.
        m_VertexIdList->SetId(pixelID, points->InsertNextPoint(cartesianCoordinates.GetDataPointer()));

        if (m_GenerateTriangularMesh)
        {
          if((i >= 1) && (j >= 1))
          {
            //This little piece of art explains the ID's:
            //
            // P(x_1y_1)---P(xy_1)
            // |           |
            // |           |
            // |           |
            // P(x_1y)-----P(xy)
            //
            //We can only start triangulation if we are at vertex (1,1),
            //because we need the other 3 vertices near this one.
            //To go one pixel line back in the image array, we have to
            //subtract 1x xDimension.
            vtkIdType xy = pixelID;
            vtkIdType x_1y = pixelID-1;
            vtkIdType xy_1 = pixelID-xDimension;
            vtkIdType x_1y_1 = xy_1-1;

            //Find the corresponding vertex ID's in the saved vertexIdList:
            vtkIdType xyV = m_VertexIdList->GetId(xy);
            vtkIdType x_1yV = m_VertexIdList->GetId(x_1y);
            vtkIdType xy_1V = m_VertexIdList->GetId(xy_1);
            vtkIdType x_1y_1V = m_VertexIdList->GetId(x_1y_1);

            if (isPointValid[xy]&&isPointValid[x_1y]&&isPointValid[x_1y_1]&&isPointValid[xy_1]) // check if points of cell are valid
            {
              double pointXY[3], pointX_1Y[3], pointXY_1[3], pointX_1Y_1[3];

              points->GetPoint(xyV, pointXY);
              points->GetPoint(x_1yV, pointX_1Y);
              points->GetPoint(xy_1V, pointXY_1);
              points->GetPoint(x_1y_1V, pointX_1Y_1);

              if( (mitk::Equal(m_TriangulationThreshold, 0.0)) || ((vtkMath::Distance2BetweenPoints(pointXY, pointX_1Y) <= m_TriangulationThreshold)
                                                                   && (vtkMath::Distance2BetweenPoints(pointXY, pointXY_1) <= m_TriangulationThreshold)
                                                                   && (vtkMath::Distance2BetweenPoints(pointX_1Y, pointX_1Y_1) <= m_TriangulationThreshold)
                                                                   && (vtkMath::Distance2BetweenPoints(pointXY_1, pointX_1Y_1) <= m_TriangulationThreshold)))
              {
                polys->InsertNextCell(3);
                polys->InsertCellPoint(x_1yV);
                polys->InsertCellPoint(xyV);
                polys->InsertCellPoint(x_1y_1V);

                polys->InsertNextCell(3);
                polys->InsertCellPoint(x_1y_1V);
                polys->InsertCellPoint(xyV);
                polys->InsertCellPoint(xy_1V);
              }
              else
              {
                //We dont want triangulation, but we want to keep the vertex
                vertices->InsertNextCell(1);
                vertices->InsertCellPoint(xyV);
              }
            }

            //Scalar values are necessary for mapping colors/texture onto the surface
            if (scalarFloatData)
            {
              scalarArray->InsertTuple1(m_VertexIdList->GetId(pixelID), scalarFloatData[pixelID]);
            }
            //These Texture Coordinates will map color pixel and vertices 1:1 (e.g. for Kinect).
            ScalarType xNorm = (((ScalarType)i)/xDimension);// correct video texture scale for kinect
            ScalarType yNorm = ((ScalarType)j)/yDimension; //don't flip. we don't need to flip.
            textureCoords->InsertTuple2(m_VertexIdList->GetId(pixelID), xNorm, yNorm);
          }
        }
        else
        {
          //We dont want triangulation, we only want vertices
          vertices->InsertNextCell(1);
          vertices->InsertCellPoint(m_VertexIdList->GetId(pixelID));
        }
      }
    }
  }

  vtkSmartPointer<vtkPolyData> mesh = vtkSmartPointer<vtkPolyData>::New();
  mesh->SetPoints(points);
  mesh->SetPolys(polys);
  mesh->SetVerts(vertices);
  //Pass the scalars to the polydata (if they were set).
  if (scalarArray->GetNumberOfTuples()>0)
  {
    mesh->GetPointData()->SetScalars(scalarArray);
  }
  //Pass the TextureCoords to the polydata anyway (to save them).
  mesh->GetPointData()->SetTCoords(textureCoords);
  output->SetVtkPolyData(mesh);
}

void mitk::ToFDistanceImageToSurfaceFilter::CreateOutputsForAllInputs()
{
  this->SetNumberOfOutputs(this->GetNumberOfInputs());  // create outputs for all inputs
  for (unsigned int idx = 0; idx < this->GetNumberOfOutputs(); ++idx)
    if (this->GetOutput(idx) == NULL)
    {
      DataObjectPointer newOutput = this->MakeOutput(idx);
      this->SetNthOutput(idx, newOutput);
    }
  this->Modified();
}

void mitk::ToFDistanceImageToSurfaceFilter::GenerateOutputInformation()
{
  this->GetOutput();
  itkDebugMacro(<<"GenerateOutputInformation()");

}

void mitk::ToFDistanceImageToSurfaceFilter::SetScalarImage(IplImage* iplScalarImage)
{
  this->m_IplScalarImage = iplScalarImage;
  this->Modified();
}

IplImage* mitk::ToFDistanceImageToSurfaceFilter::GetScalarImage()
{
  return this->m_IplScalarImage;
}

void mitk::ToFDistanceImageToSurfaceFilter::SetTextureImageWidth(int width)
{
  this->m_TextureImageWidth = width;
}

void mitk::ToFDistanceImageToSurfaceFilter::SetTextureImageHeight(int height)
{
  this->m_TextureImageHeight = height;
}

void mitk::ToFDistanceImageToSurfaceFilter::SetTriangulationThreshold(double triangulationThreshold)
{
  //vtkMath::Distance2BetweenPoints returns the squared distance between two points and
  //hence we square m_TriangulationThreshold in order to save run-time.
  this->m_TriangulationThreshold = triangulationThreshold*triangulationThreshold;
}
<|endoftext|>


<commit_before>#include <Windows.h>
#include <stdint.h> // Types indpendent de la plateforme

// Pour bien comprendre la diffrence de fonctionnement des variables statiques en C en fonction du scope
#define internal static // fonctions non visible depuis l'extrieur de ce fichier
#define local_persist static     // variable visibles juste dans le scope o elle dfinie
#define global_variable static   // variable visible dans tous le fichiers (globale)

typedef unsigned char uint8;
typedef uint8_t uint8; // comme un unsigned char, un 8 bits
typedef int16_t uint16;
typedef int32_t uint32;
typedef int64_t uint64;

// variable globale pour le moment, on grera autrement plus tard
global_variable bool Running;
global_variable BITMAPINFO BitmapInfo;
global_variable void *BitmapMemory;
global_variable int BitmapWidth;
global_variable int BitmapHeight;
global_variable int BytesPerPixel = 4;

internal void
RenderWeirdGradient(int XOffset, int YOffset)
{
  int Width = BitmapWidth;
  int Height = BitmapHeight;

  int Pitch = Width * BytesPerPixel; // Pitch reprsente la taille d'une ligne en octets
  uint8 *Row = (uint8 *)BitmapMemory; // on va se dplacer dans la mmoire par pas de 8 bits
  for (int Y = 0; Y < BitmapHeight; ++Y)
  {
    uint32 *Pixel = (uint32 *)Row; // Pixel par pixel, on commence par le premier de la ligne
    for (int X = 0; X < BitmapWidth; ++X)
    {
      /*
        Pixels en little endian architecture
                             0  1  2  3 ...
        Pixels en mmoire : 00 00 00 00 ...
        Couleur             BB GG RR XX
        en hexa: 0xXXRRGGBB
      */
      uint8 Blue = (X + XOffset);
      uint8 Green = (Y + YOffset);
      // *Pixel = 0xFF00FF00;
      *Pixel = ((Green << 8) | Blue); // ce qui quivaut en hexa  0x00BBGG00
      ++Pixel; // Faon de faire si on prend pixel par pixel
    }
    Row += Pitch; // Ligne suivante
  }
}

/**
 * DIB: Device Independent Bitmap
 **/
internal void
Win32ResizeDIBSection(int Width, int Height)
{
  if (BitmapMemory)
  {
    VirtualFree(BitmapMemory, 0, MEM_RELEASE); // cf. VirtualProtect, utile pour debug
  }

  BitmapWidth = Width;
  BitmapHeight = Height;

  BitmapInfo.bmiHeader.biSize = sizeof(BitmapInfo.bmiHeader);
  BitmapInfo.bmiHeader.biWidth = BitmapWidth;
  BitmapInfo.bmiHeader.biHeight = -BitmapHeight; // Atention au sens des coordonnes
  BitmapInfo.bmiHeader.biPlanes = 1;
  BitmapInfo.bmiHeader.biBitCount = 32;
  BitmapInfo.bmiHeader.biCompression = BI_RGB;

  int BitmapMemorySize = (BitmapWidth * BitmapHeight) * BytesPerPixel;
  BitmapMemory = VirtualAlloc(0, BitmapMemorySize, MEM_COMMIT, PAGE_READWRITE); // cf. aussi HeapAlloc
}

internal void
Win32UpdateWindow(HDC DeviceContext, RECT *ClientRect, int X, int Y, int Width, int Height)
{
  int WindowWidth = ClientRect->right - ClientRect->left;
  int WindowHeight = ClientRect->bottom - ClientRect->top;
  StretchDIBits( // copie d'un rectangle vers un autre (scaling si ncessaire, bit oprations...)
    DeviceContext,
    /*X, Y, Width, Height,
    X, Y, Width, Height,*/
    0, 0, BitmapWidth, BitmapHeight,
    0, 0, WindowWidth, WindowHeight,
    BitmapMemory,
    &BitmapInfo,
    DIB_RGB_COLORS,
    SRCCOPY // BitBlt: bit-block transfer of the color data => voir les autres modes dans la MSDN
  );
}

LRESULT CALLBACK
Win32MainWindowCallback(
  HWND Window,
  UINT Message,
  WPARAM WParam,
  LPARAM LParam)
{
  LRESULT Result = 0;
  switch(Message)
  {
    case WM_SIZE:
      {
        RECT ClientRect;
        GetClientRect(Window, &ClientRect);
        int Width = ClientRect.right - ClientRect.left;
        int Height = ClientRect.bottom - ClientRect.top;
        Win32ResizeDIBSection(Width, Height);
        OutputDebugStringA("WM_SIZE\n");
      }
      break;
    case WM_DESTROY:
      {
        // PostQuitMessage(0); // Va permettre de sortir de la boucle infinie en dessous
        Running = false;
        OutputDebugStringA("WM_DESTROY\n");
      }
      break;
    case WM_CLOSE:
      {
        // DestroyWindow(Window);
        Running = false;
        OutputDebugStringA("WM_CLOSE\n");
      }
      break;
    case WM_ACTIVATEAPP:
      {
        OutputDebugStringA("WM_ACTIVATEAPP\n");
      }
      break;
    case WM_PAINT:
      {
        PAINTSTRUCT Paint;
        HDC DeviceContext = BeginPaint(Window, &Paint);
        int X = Paint.rcPaint.left;
        int Y = Paint.rcPaint.top;
        int Width = Paint.rcPaint.right - Paint.rcPaint.left;
        int Height = Paint.rcPaint.bottom - Paint.rcPaint.top;

        RECT ClientRect;
        GetClientRect(Window, &ClientRect);

        Win32UpdateWindow(DeviceContext, &ClientRect, X, Y, Width, Height);
        EndPaint(Window, &Paint);
      }
      break;
    default:
      {
        // OutputDebugStringA("default\n");
        Result = DefWindowProc(Window, Message, WParam, LParam);
      }
      break;
  }
  return(Result);
}

int CALLBACK
WinMain(
  HINSTANCE Instance,
  HINSTANCE PrevInstance,
  LPSTR CommandLine,
  int ShowCode)
{
  // Cration de la fentre principale
  WNDCLASSA WindowClass = {}; // initialisation par dfaut, ANSI version de WNDCLASSA
  // On ne configure que les membres que l'on veut
  WindowClass.style = CS_OWNDC|CS_HREDRAW|CS_VREDRAW;
  WindowClass.lpfnWndProc = Win32MainWindowCallback;
  WindowClass.hInstance = Instance;
  // WindowClass.hIcon;
  WindowClass.lpszClassName = "FaitmainHerosWindowClass"; // nom pour retrouver la fentre

  // Ouverture de la fentre
  if (RegisterClassA(&WindowClass))
  {
    HWND Window = CreateWindowExA( // ANSI version de CreateWindowEx
      0, // dwExStyle : options de la fentre
      WindowClass.lpszClassName,
      "FaitmainHeros",
      WS_OVERLAPPEDWINDOW|WS_VISIBLE, //dwStyle : overlapped window, visible par dfaut
      CW_USEDEFAULT, // X
      CW_USEDEFAULT, // Y
      CW_USEDEFAULT, // nWidth
      CW_USEDEFAULT, // nHeight
      0, // hWndParent : 0 pour dire que c'est une fentre top
      0, // hMenu : 0 pour dire pas de menu
      Instance,
      0 // Pas de passage de paramtres  la fentre
    );
    if (Window)
    {
      int XOffset = 0;
      int YOffset = 0;
      MSG Message;
      Running = true;
      while (Running) // boucle infinie pour traiter tous les messages
      {
        while(PeekMessageA(&Message, 0, 0, 0, PM_REMOVE)) // On utilise PeekMessage au lieu de GetMessage qui est bloquant
        {
          if (Message.message == WM_QUIT) Running = false;
          TranslateMessage(&Message); // On demande  Windows de traiter le message
          DispatchMessage(&Message); // Envoie le message au main WindowCallback, que l'on a dfini et dclar au dessus
        }

        // Grce  PeekMessage on a tout le temps CPU que l'on veut et on peut dessiner ici
        RenderWeirdGradient(XOffset, YOffset);
        ++XOffset;

        // On doit alors crire dans la fentre  chaque fois que l'on veut rendre
        // On en fera une fonction propre
        HDC DeviceContext = GetDC(Window);
        RECT ClientRect;
        GetClientRect(Window, &ClientRect);
        int WindowWidth = ClientRect.right - ClientRect.left;
        int WindowHeight = ClientRect.bottom - ClientRect.top;
        Win32UpdateWindow(DeviceContext, &ClientRect, 0, 0, WindowWidth, WindowHeight);
        ReleaseDC(Window, DeviceContext);
      }
    }
    else
    {
      OutputDebugStringA("Error: CreateWindowEx\n");
    }
  }
  else
  {
    OutputDebugStringA("Error: RegisterClass\n");
  }

  return(0);
};<commit_msg>Refactorisation du code du backbuffer<commit_after>#include <Windows.h>
#include <stdint.h> // Types indpendent de la plateforme

// Pour bien comprendre la diffrence de fonctionnement des variables statiques en C en fonction du scope
#define internal static // fonctions non visible depuis l'extrieur de ce fichier
#define local_persist static     // variable visibles juste dans le scope o elle dfinie
#define global_variable static   // variable visible dans tous le fichiers (globale)

typedef unsigned char uint8;
typedef uint8_t uint8; // comme un unsigned char, un 8 bits
typedef int16_t uint16;
typedef int32_t uint32;
typedef int64_t uint64;

/* Struct sui reprsente un backbuffer qui nous permet de dessiner */
struct win32_offscreen_buffer {
  BITMAPINFO Info;
  void *Memory;
  int Width;
  int Height;
  int BytesPerPixel;
  int Pitch; // Pitch reprsente la taille d'une ligne en octets
};

// variables globales pour le moment, on grera autrement plus tard
global_variable bool Running;
global_variable win32_offscreen_buffer GlobalBackBuffer;

struct win32_window_dimension
{
  int Width;
  int Height;
};

win32_window_dimension
GetWindowDimension(HWND Window) {
  win32_window_dimension Result;
  RECT ClientRect;
  GetClientRect(Window, &ClientRect);
  Result.Width = ClientRect.right - ClientRect.left;
  Result.Height = ClientRect.bottom - ClientRect.top;
  return(Result);
}

internal void
RenderWeirdGradient(win32_offscreen_buffer *Buffer, int XOffset, int YOffset)
{
  uint8 *Row = (uint8 *)Buffer->Memory; // on va se dplacer dans la mmoire par pas de 8 bits
  for (int Y = 0; Y < Buffer->Height; ++Y)
  {
    uint32 *Pixel = (uint32 *)Row; // Pixel par pixel, on commence par le premier de la ligne
    for (int X = 0; X < Buffer->Width; ++X)
    {
      /*
        Pixels en little endian architecture
                             0  1  2  3 ...
        Pixels en mmoire : 00 00 00 00 ...
        Couleur             BB GG RR XX
        en hexa: 0xXXRRGGBB
      */
      uint8 Blue = (X + XOffset);
      uint8 Green = (Y + YOffset);
      // *Pixel = 0xFF00FF00;
      *Pixel = ((Green << 8) | Blue); // ce qui quivaut en hexa  0x00BBGG00
      ++Pixel; // Faon de faire si on prend pixel par pixel
    }
    Row += Buffer->Pitch; // Ligne suivante
  }
}

/**
 * DIB: Device Independent Bitmap
 **/
internal void
Win32ResizeDIBSection(win32_offscreen_buffer *Buffer, int Width, int Height)
{
  if (Buffer->Memory)
  {
    VirtualFree(Buffer->Memory, 0, MEM_RELEASE); // cf. VirtualProtect, utile pour debug
  }

  Buffer->Width = Width;
  Buffer->Height = Height;
  Buffer->BytesPerPixel = 4;

  Buffer->Info.bmiHeader.biSize = sizeof(Buffer->Info.bmiHeader);
  Buffer->Info.bmiHeader.biWidth = Buffer->Width;
  Buffer->Info.bmiHeader.biHeight = -Buffer->Height; // Atention au sens des coordonnes
  Buffer->Info.bmiHeader.biPlanes = 1;
  Buffer->Info.bmiHeader.biBitCount = 32;
  Buffer->Info.bmiHeader.biCompression = BI_RGB;

  int BitmapMemorySize = (Buffer->Width * Buffer->Height) * Buffer->BytesPerPixel;
  Buffer->Memory = VirtualAlloc(0, BitmapMemorySize, MEM_COMMIT, PAGE_READWRITE); // cf. aussi HeapAlloc

  Buffer->Pitch = Width * Buffer->BytesPerPixel;
}

/**
 * Ici au dbut on passait ClientRect par rfrence avec un pointeur (*ClientRect)
 * cependant comme la structure est petite le passer par valeur est suffisant
 **/
internal void
Win32DisplayBufferInWindow(
  HDC DeviceContext, int WindowWidth, int WindowHeight,
  win32_offscreen_buffer *Buffer,
  int X, int Y, int Width, int Height)
{
  StretchDIBits( // copie d'un rectangle vers un autre (scaling si ncessaire, bit oprations...)
    DeviceContext,
    /*X, Y, Width, Height,
    X, Y, Width, Height,*/
    0, 0, Buffer->Width, Buffer->Height,
    0, 0, WindowWidth, WindowHeight,
    Buffer->Memory,
    &Buffer->Info,
    DIB_RGB_COLORS,
    SRCCOPY // BitBlt: bit-block transfer of the color data => voir les autres modes dans la MSDN
  );
}

LRESULT CALLBACK
Win32MainWindowCallback(
  HWND Window,
  UINT Message,
  WPARAM WParam,
  LPARAM LParam)
{
  LRESULT Result = 0;
  switch(Message)
  {
    case WM_SIZE:
      {
        win32_window_dimension Dimension = GetWindowDimension(Window);
        Win32ResizeDIBSection(&GlobalBackBuffer, Dimension.Width, Dimension.Height);
        OutputDebugStringA("WM_SIZE\n");
      }
      break;
    case WM_DESTROY:
      {
        // PostQuitMessage(0); // Va permettre de sortir de la boucle infinie en dessous
        Running = false;
        OutputDebugStringA("WM_DESTROY\n");
      }
      break;
    case WM_CLOSE:
      {
        // DestroyWindow(Window);
        Running = false;
        OutputDebugStringA("WM_CLOSE\n");
      }
      break;
    case WM_ACTIVATEAPP:
      {
        OutputDebugStringA("WM_ACTIVATEAPP\n");
      }
      break;
    case WM_PAINT:
      {
        PAINTSTRUCT Paint;
        HDC DeviceContext = BeginPaint(Window, &Paint);
        int X = Paint.rcPaint.left;
        int Y = Paint.rcPaint.top;
        int Width = Paint.rcPaint.right - Paint.rcPaint.left;
        int Height = Paint.rcPaint.bottom - Paint.rcPaint.top;

        win32_window_dimension Dimension = GetWindowDimension(Window);
        Win32DisplayBufferInWindow(DeviceContext, Dimension.Width, Dimension.Height, &GlobalBackBuffer, X, Y, Width, Height);
        EndPaint(Window, &Paint);
      }
      break;
    default:
      {
        // OutputDebugStringA("default\n");
        Result = DefWindowProc(Window, Message, WParam, LParam);
      }
      break;
  }
  return(Result);
}

int CALLBACK
WinMain(
  HINSTANCE Instance,
  HINSTANCE PrevInstance,
  LPSTR CommandLine,
  int ShowCode)
{
  // Cration de la fentre principale
  WNDCLASSA WindowClass = {}; // initialisation par dfaut, ANSI version de WNDCLASSA
  // On ne configure que les membres que l'on veut
  WindowClass.style = CS_HREDRAW|CS_VREDRAW; // indique que l'on veut rafraichir la fentre entire lors d'un resize (horizontal et vertical)
  WindowClass.lpfnWndProc = Win32MainWindowCallback;
  WindowClass.hInstance = Instance;
  // WindowClass.hIcon;
  WindowClass.lpszClassName = "FaitmainHerosWindowClass"; // nom pour retrouver la fentre

  // Ouverture de la fentre
  if (RegisterClassA(&WindowClass))
  {
    HWND Window = CreateWindowExA( // ANSI version de CreateWindowEx
      0, // dwExStyle : options de la fentre
      WindowClass.lpszClassName,
      "FaitmainHeros",
      WS_OVERLAPPEDWINDOW|WS_VISIBLE, //dwStyle : overlapped window, visible par dfaut
      CW_USEDEFAULT, // X
      CW_USEDEFAULT, // Y
      CW_USEDEFAULT, // nWidth
      CW_USEDEFAULT, // nHeight
      0, // hWndParent : 0 pour dire que c'est une fentre top
      0, // hMenu : 0 pour dire pas de menu
      Instance,
      0 // Pas de passage de paramtres  la fentre
    );
    if (Window)
    {
      int XOffset = 0;
      int YOffset = 0;
      MSG Message;
      Running = true;
      while (Running) // boucle infinie pour traiter tous les messages
      {
        while(PeekMessageA(&Message, 0, 0, 0, PM_REMOVE)) // On utilise PeekMessage au lieu de GetMessage qui est bloquant
        {
          if (Message.message == WM_QUIT) Running = false;
          TranslateMessage(&Message); // On demande  Windows de traiter le message
          DispatchMessage(&Message); // Envoie le message au main WindowCallback, que l'on a dfini et dclar au dessus
        }

        // Grce  PeekMessage on a tout le temps CPU que l'on veut et on peut dessiner ici
        RenderWeirdGradient(&GlobalBackBuffer, XOffset, YOffset);
        ++XOffset;

        // On doit alors crire dans la fentre  chaque fois que l'on veut rendre
        // On en fera une fonction propre
        HDC DeviceContext = GetDC(Window);
        win32_window_dimension Dimension = GetWindowDimension(Window);
        Win32DisplayBufferInWindow(DeviceContext, Dimension.Width, Dimension.Height, &GlobalBackBuffer, 0, 0, Dimension.Width, Dimension.Height);
        ReleaseDC(Window, DeviceContext);
      }
    }
    else
    {
      OutputDebugStringA("Error: CreateWindowEx\n");
    }
  }
  else
  {
    OutputDebugStringA("Error: RegisterClass\n");
  }

  return(0);
};<|endoftext|>


<commit_before>/* Copyright 2016-2017 CyberTech Labs Ltd.
 *
 * 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 <QtCore/QTimer>
#include <QtCore/QEventLoop>

#include <QtCore/QThread>

#include <trikKitInterpreterCommon/trikbrick.h>

#include <utils/abstractTimer.h>
#include <kitBase/robotModel/robotModelUtils.h>
#include <trikKit/robotModel/parts/trikShell.h>
#include <trikKit/robotModel/parts/trikLineSensor.h>
#include <kitBase/robotModel/robotParts/gyroscopeSensor.h>
#include <kitBase/robotModel/robotParts/encoderSensor.h>
#include <kitBase/robotModel/robotParts/random.h>
#include <kitBase/robotModel/robotParts/random.h>
#include <twoDModel/robotModel/parts/marker.h>
#include <twoDModel/engine/model/timeline.h>
///todo: temporary
#include <trikKitInterpreterCommon/robotModel/twoD/parts/twoDDisplay.h>

using namespace trik;

TrikBrick::TrikBrick(const QSharedPointer<robotModel::twoD::TrikTwoDRobotModel> &model)
	: mTwoDRobotModel(model)
	, mDisplay(model)
	, mKeys(model)
	, mSensorUpdater(model->timeline().produceTimer())
{
	connect(this, &TrikBrick::log, this, &TrikBrick::printToShell);
	mSensorUpdater->setRepeatable(true);
	mSensorUpdater->setInterval(model->updateIntervalForInterpretation()); // seems to be x2 of timeline tick
	connect(mSensorUpdater.data(), &utils::AbstractTimer::timeout, [model](){
		model->updateSensorsValues(); /// @todo: maybe connect to model directly?
	});
}

TrikBrick::~TrikBrick()
{
	qDeleteAll(mMotors);
	qDeleteAll(mSensors);
	qDeleteAll(mEncoders);
	qDeleteAll(mLineSensors);
	qDeleteAll(mTimers);
}

void TrikBrick::reset()
{
	mKeys.reset();///@todo: reset motos/device maps?
	//mDisplay.reset(); - is actually needed? Crashes app at exit
	emit stopWaiting();
	for (const auto &m : mMotors) {
		m->powerOff();
	}
	for (const auto &e : mEncoders) {
		e->reset();
	}
	for (const auto &t : mTimers) {
		t->stop();
	}
	qDeleteAll(mTimers);
	mTimers.clear();
	QMetaObject::invokeMethod(mSensorUpdater.data(), "stop"); // failproof against timer manipulation in another thread
}

void TrikBrick::printToShell(const QString &msg)
{
	using namespace kitBase::robotModel;
	using namespace trik::robotModel;
	parts::TrikShell* sh = RobotModelUtils::findDevice<parts::TrikShell>(*mTwoDRobotModel, "ShellPort");
	if (sh == nullptr) {
		qDebug("Error: 2d model shell part was not found");
		return;
	}
	sh->print(msg);
}

void TrikBrick::init()
{
	mDisplay.init();
//	for (const auto &m : mMotors) {
//		m->powerOff();
//	}
//	for (const auto &e : mEncoders) {
//		e->read();
//	}
//	for (const auto &s : mSensors) {
//		s->read();
//	}
	mTwoDRobotModel->updateSensorsValues();
	mMotors.clear(); // needed? reset?
	mSensors.clear();
	mEncoders.clear();
	mKeys.init();
	mGyroscope.reset(); // for some reason it won't reconnect to the robot parts otherwise.
	QMetaObject::invokeMethod(mSensorUpdater.data(), "start"); // failproof against timer manipulation in another thread
	//mSensorUpdater.start();
}

void TrikBrick::setCurrentDir(const QString &dir)
{
	mCurrentDir = QFileInfo(dir).dir(); // maybe can be constructed directly
}

void TrikBrick::setCurrentInputs(const QString &f)
{
	mIsExcerciseMode = true;
	if (f.isEmpty()) {
		return; // no inputs has been passed, no need to complain
	}
	QString file(f);
	QFile in(file);
	if (!in.open(QIODevice::ReadOnly | QIODevice::Text)) {
		emit warning(tr("Trying to read from file %1 failed").arg(file)); // todo: remove? It's only in exercise.
		//not really an error, usually
	}

	QStringList result;

	while (!in.atEnd()) {
		const auto line = in.readLine();
		result << QString::fromUtf8(line);
	}

	mInputs = result;
}

void TrikBrick::say(const QString &msg) {
	using namespace kitBase::robotModel;
	using namespace trik::robotModel;
	parts::TrikShell* sh = RobotModelUtils::findDevice<parts::TrikShell>(*mTwoDRobotModel, "ShellPort");
	if (sh == nullptr) {
		qDebug("Error: 2d model shell part was not found");
		return;
	}
	QMetaObject::invokeMethod(sh, "say", Q_ARG(const QString &, msg));
}

void TrikBrick::stop() {
	/// @todo: properly implement this?
	mTwoDRobotModel->stopRobot();
//	for (const auto &m : mMotors) {
//		m->powerOff();
//	}
//	for (const auto &e : mEncoders) {
//		e->reset();
//	}
}

trikControl::MotorInterface *TrikBrick::motor(const QString &port)
{
	using namespace kitBase::robotModel;
	if (!mMotors.contains(port)) {
		robotParts::Motor * mot =
				RobotModelUtils::findDevice<robotParts::Motor>(*mTwoDRobotModel, port);
		if (mot == nullptr) {
			emit error(tr("No configured motor on port: %1").arg(port));
			return nullptr;
		}
		mMotors[port] = new TrikMotorEmu(mot);
	}
	return mMotors[port];
}

trikControl::MarkerInterface *TrikBrick::marker()
{
	kitBase::robotModel::PortInfo markerPort = kitBase::robotModel::RobotModelUtils::findPort(*mTwoDRobotModel
			, "MarkerPort"
			, kitBase::robotModel::Direction::output);
	if (markerPort.isValid()) {
		using Marker = twoDModel::robotModel::parts::Marker;
		Marker* marker = kitBase::robotModel::RobotModelUtils::findDevice<Marker>(*mTwoDRobotModel, markerPort);
		mTrikProxyMarker.reset(new TrikProxyMarker(marker));
		return mTrikProxyMarker.data();
	}

	return nullptr;
}

trikControl::SensorInterface *TrikBrick::sensor(const QString &port)
{
	//testing
	using namespace kitBase::robotModel;
	if (!mSensors.contains(port)) {
		robotParts::ScalarSensor * sens =
				RobotModelUtils::findDevice<robotParts::ScalarSensor>(*mTwoDRobotModel, port);
		if (sens == nullptr) {
			emit error(tr("No configured sensor on port: %1").arg(port));
			return nullptr;
		}
		mSensors[port] = new TrikSensorEmu(sens);
	}
	return mSensors[port];
}

QStringList TrikBrick::motorPorts(trikControl::MotorInterface::Type type) const
{
	Q_UNUSED(type)
//	QLOG_INFO() << "Motor type is ignored";
	return mMotors.keys();
}

QStringList TrikBrick::sensorPorts(trikControl::SensorInterface::Type type) const
{
	Q_UNUSED(type)
//	QLOG_INFO() << "Sensor type is ignored";
	return mMotors.keys();
}

QStringList TrikBrick::encoderPorts() const {
	return mEncoders.keys();
}

trikControl::VectorSensorInterface *TrikBrick::accelerometer() {
	using namespace kitBase::robotModel;
	if (mAccelerometer.isNull()) {
		auto a = RobotModelUtils::findDevice<robotParts::AccelerometerSensor>(*mTwoDRobotModel
				, "AccelerometerPort");
		if (a == nullptr) {
			emit error(tr("No configured accelerometer"));
			return nullptr;
		}
		mAccelerometer.reset(new TrikAccelerometerAdapter(a));
	}

	return mAccelerometer.data();
}

trikControl::GyroSensorInterface *TrikBrick::gyroscope() {
	using namespace kitBase::robotModel;
	if (mGyroscope.isNull()) {
		auto a = RobotModelUtils::findDevice<robotParts::GyroscopeSensor>(*mTwoDRobotModel
				, "GyroscopePort");
		if (a == nullptr) {
			emit error(tr("No configured gyroscope"));
			return nullptr;
		}
		mGyroscope.reset(new TrikGyroscopeAdapter(a, mTwoDRobotModel));
	}

	return mGyroscope.data();
}

trikControl::LineSensorInterface *TrikBrick::lineSensor(const QString &port) {
	using namespace trik::robotModel::parts;
	using namespace kitBase::robotModel;
	if (port == "video0") {
		return lineSensor("LineSensorPort"); // seems to be the case for 2d model
	}

	if (!mLineSensors.contains(port)) {
		TrikLineSensor * sens =
				RobotModelUtils::findDevice<TrikLineSensor>(*mTwoDRobotModel, port);
		if (sens == nullptr) {
			emit error(tr("No configured LineSensor on port: %1").arg(port));
			return nullptr;
		}
		mLineSensors[port] = new TrikLineSensorAdapter(sens);
	}

	return mLineSensors[port];
}

trikControl::EncoderInterface *TrikBrick::encoder(const QString &port) {
	using namespace kitBase::robotModel;
	if (!mEncoders.contains(port)) {
		robotParts::EncoderSensor * enc =
				RobotModelUtils::findDevice<robotParts::EncoderSensor>(*mTwoDRobotModel, port);
		if (enc == nullptr) {
			emit error(tr("No configured encoder on port: %1").arg(port));
			return nullptr;
		}

		mEncoders[port] = new TrikEncoderAdapter(enc->port(), mTwoDRobotModel->engine());
	}

	return mEncoders[port];
}

trikControl::DisplayInterface *TrikBrick::display()
{
//	trik::robotModel::parts::TrikDisplay * const display =
//			kitBase::robotModel::RobotModelUtils::findDevice<trik::robotModel::parts::TrikDisplay>(*mTwoDRobotModel
//					, "DisplayPort");
//	if (display) {
//		bool res = QMetaObject::invokeMethod(display,
//		"drawSmile",
//		Qt::QueuedConnection, // connection type, auto?
//		Q_ARG(bool, false));
//		//display->drawSmile(false);
//		printf(res ? "true" : "false");
//	}
//	return nullptr;
	return &mDisplay;
}

trikControl::LedInterface *TrikBrick::led() {
	using namespace trik::robotModel::parts;
	using namespace kitBase::robotModel;
	if (mLed.isNull()) {
		auto l = RobotModelUtils::findDevice<TrikLed>(*mTwoDRobotModel, "LedPort");
		if (l == nullptr) {
			emit error(tr("No configured led"));
			return nullptr;
		}
		mLed.reset(new TrikLedAdapter(l));
	}

	return mLed.data();
}

int TrikBrick::random(int from, int to)
{
	using namespace kitBase::robotModel;
	auto r = RobotModelUtils::findDevice<robotParts::Random>(*mTwoDRobotModel, "RandomPort");
	// maybe store it later, like the others
	if (!r) {
		emit error(tr("No cofigured random device"));
		return -1;
	}

	return r->random(from, to);
}

void TrikBrick::wait(int milliseconds)
{
	auto timeline = dynamic_cast<twoDModel::model::Timeline *> (&mTwoDRobotModel->timeline());

	if (timeline->isStarted()) {
		QSharedPointer<utils::AbstractTimer> t(timeline->produceTimer());
		QTimer abortTimer;
		QMetaObject::Connection abortConnection;
		QSharedPointer<QEventLoop> loop (new QEventLoop());

		auto mainHandler = [=]() {
			disconnect(abortConnection);
			disconnect(this, &TrikBrick::stopWaiting, nullptr, nullptr);
			disconnect(timeline, &twoDModel::model::Timeline::beforeStop, nullptr, nullptr);
			disconnect(t.data(), &utils::AbstractTimer::timeout, nullptr, nullptr);
			loop->quit();
		};

		auto abortHandler = [=]() {
			if (!timeline->isStarted()) {
				mainHandler();
			}
		};

		connect(t.data(), &utils::AbstractTimer::timeout, mainHandler);
		connect(this, &TrikBrick::stopWaiting, mainHandler);

		// timers that are produced by produceTimer() doesn't use stop singal
		// be careful, one who use just utils::AbstractTimer can stuck
		connect(timeline, &twoDModel::model::Timeline::beforeStop, mainHandler);
		abortConnection = connect(&abortTimer, &QTimer::timeout, abortHandler);

		// because timer is depends on twoDModel::model::Timeline
		if (timeline->isStarted()) {
			t->start(milliseconds);
			abortTimer.start(10);
			loop->exec();
		} else {
			mainHandler();
		}
	}
}

quint64 TrikBrick::time() const
{
	return mTwoDRobotModel->timeline().timestamp();
}

QStringList TrikBrick::readAll(const QString &path)
{
	if (mIsExcerciseMode) {
		return mInputs;
	}
	//if (mCurrentDir) todo: check that the current working dir is a save dir
	QFileInfo normalizedPath(mCurrentDir.absoluteFilePath(path)); // absoluteDir?
	QString file = normalizedPath.filePath();
	QFile in(file);
	if (!in.open(QIODevice::ReadOnly | QIODevice::Text)) {
		emit error(tr("Trying to read from file %1 failed").arg(file));
		return {};
	}

	QStringList result;

	while (!in.atEnd()) {
		const auto line = in.readLine();
		result << QString::fromUtf8(line);
	}

	return result;
}

utils::AbstractTimer *TrikBrick::timer(int milliseconds)
{
	utils::AbstractTimer *result = mTwoDRobotModel->timeline().produceTimer();
	mTimers.append(result);
	result->setRepeatable(true); // seems to be the case
	result->start(milliseconds);
	return result;
}

<commit_msg>"this" in connect was returned<commit_after>/* Copyright 2016-2017 CyberTech Labs Ltd.
 *
 * 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 <QtCore/QTimer>
#include <QtCore/QEventLoop>

#include <QtCore/QThread>

#include <trikKitInterpreterCommon/trikbrick.h>

#include <utils/abstractTimer.h>
#include <kitBase/robotModel/robotModelUtils.h>
#include <trikKit/robotModel/parts/trikShell.h>
#include <trikKit/robotModel/parts/trikLineSensor.h>
#include <kitBase/robotModel/robotParts/gyroscopeSensor.h>
#include <kitBase/robotModel/robotParts/encoderSensor.h>
#include <kitBase/robotModel/robotParts/random.h>
#include <kitBase/robotModel/robotParts/random.h>
#include <twoDModel/robotModel/parts/marker.h>
#include <twoDModel/engine/model/timeline.h>
///todo: temporary
#include <trikKitInterpreterCommon/robotModel/twoD/parts/twoDDisplay.h>

using namespace trik;

TrikBrick::TrikBrick(const QSharedPointer<robotModel::twoD::TrikTwoDRobotModel> &model)
	: mTwoDRobotModel(model)
	, mDisplay(model)
	, mKeys(model)
	, mSensorUpdater(model->timeline().produceTimer())
{
	connect(this, &TrikBrick::log, this, &TrikBrick::printToShell);
	mSensorUpdater->setRepeatable(true);
	mSensorUpdater->setInterval(model->updateIntervalForInterpretation()); // seems to be x2 of timeline tick
	connect(mSensorUpdater.data(), &utils::AbstractTimer::timeout, [model](){
		model->updateSensorsValues(); /// @todo: maybe connect to model directly?
	});
}

TrikBrick::~TrikBrick()
{
	qDeleteAll(mMotors);
	qDeleteAll(mSensors);
	qDeleteAll(mEncoders);
	qDeleteAll(mLineSensors);
	qDeleteAll(mTimers);
}

void TrikBrick::reset()
{
	mKeys.reset();///@todo: reset motos/device maps?
	//mDisplay.reset(); - is actually needed? Crashes app at exit
	emit stopWaiting();
	for (const auto &m : mMotors) {
		m->powerOff();
	}
	for (const auto &e : mEncoders) {
		e->reset();
	}
	for (const auto &t : mTimers) {
		t->stop();
	}
	qDeleteAll(mTimers);
	mTimers.clear();
	QMetaObject::invokeMethod(mSensorUpdater.data(), "stop"); // failproof against timer manipulation in another thread
}

void TrikBrick::printToShell(const QString &msg)
{
	using namespace kitBase::robotModel;
	using namespace trik::robotModel;
	parts::TrikShell* sh = RobotModelUtils::findDevice<parts::TrikShell>(*mTwoDRobotModel, "ShellPort");
	if (sh == nullptr) {
		qDebug("Error: 2d model shell part was not found");
		return;
	}
	sh->print(msg);
}

void TrikBrick::init()
{
	mDisplay.init();
//	for (const auto &m : mMotors) {
//		m->powerOff();
//	}
//	for (const auto &e : mEncoders) {
//		e->read();
//	}
//	for (const auto &s : mSensors) {
//		s->read();
//	}
	mTwoDRobotModel->updateSensorsValues();
	mMotors.clear(); // needed? reset?
	mSensors.clear();
	mEncoders.clear();
	mKeys.init();
	mGyroscope.reset(); // for some reason it won't reconnect to the robot parts otherwise.
	QMetaObject::invokeMethod(mSensorUpdater.data(), "start"); // failproof against timer manipulation in another thread
	//mSensorUpdater.start();
}

void TrikBrick::setCurrentDir(const QString &dir)
{
	mCurrentDir = QFileInfo(dir).dir(); // maybe can be constructed directly
}

void TrikBrick::setCurrentInputs(const QString &f)
{
	mIsExcerciseMode = true;
	if (f.isEmpty()) {
		return; // no inputs has been passed, no need to complain
	}
	QString file(f);
	QFile in(file);
	if (!in.open(QIODevice::ReadOnly | QIODevice::Text)) {
		emit warning(tr("Trying to read from file %1 failed").arg(file)); // todo: remove? It's only in exercise.
		//not really an error, usually
	}

	QStringList result;

	while (!in.atEnd()) {
		const auto line = in.readLine();
		result << QString::fromUtf8(line);
	}

	mInputs = result;
}

void TrikBrick::say(const QString &msg) {
	using namespace kitBase::robotModel;
	using namespace trik::robotModel;
	parts::TrikShell* sh = RobotModelUtils::findDevice<parts::TrikShell>(*mTwoDRobotModel, "ShellPort");
	if (sh == nullptr) {
		qDebug("Error: 2d model shell part was not found");
		return;
	}
	QMetaObject::invokeMethod(sh, "say", Q_ARG(const QString &, msg));
}

void TrikBrick::stop() {
	/// @todo: properly implement this?
	mTwoDRobotModel->stopRobot();
//	for (const auto &m : mMotors) {
//		m->powerOff();
//	}
//	for (const auto &e : mEncoders) {
//		e->reset();
//	}
}

trikControl::MotorInterface *TrikBrick::motor(const QString &port)
{
	using namespace kitBase::robotModel;
	if (!mMotors.contains(port)) {
		robotParts::Motor * mot =
				RobotModelUtils::findDevice<robotParts::Motor>(*mTwoDRobotModel, port);
		if (mot == nullptr) {
			emit error(tr("No configured motor on port: %1").arg(port));
			return nullptr;
		}
		mMotors[port] = new TrikMotorEmu(mot);
	}
	return mMotors[port];
}

trikControl::MarkerInterface *TrikBrick::marker()
{
	kitBase::robotModel::PortInfo markerPort = kitBase::robotModel::RobotModelUtils::findPort(*mTwoDRobotModel
			, "MarkerPort"
			, kitBase::robotModel::Direction::output);
	if (markerPort.isValid()) {
		using Marker = twoDModel::robotModel::parts::Marker;
		Marker* marker = kitBase::robotModel::RobotModelUtils::findDevice<Marker>(*mTwoDRobotModel, markerPort);
		mTrikProxyMarker.reset(new TrikProxyMarker(marker));
		return mTrikProxyMarker.data();
	}

	return nullptr;
}

trikControl::SensorInterface *TrikBrick::sensor(const QString &port)
{
	//testing
	using namespace kitBase::robotModel;
	if (!mSensors.contains(port)) {
		robotParts::ScalarSensor * sens =
				RobotModelUtils::findDevice<robotParts::ScalarSensor>(*mTwoDRobotModel, port);
		if (sens == nullptr) {
			emit error(tr("No configured sensor on port: %1").arg(port));
			return nullptr;
		}
		mSensors[port] = new TrikSensorEmu(sens);
	}
	return mSensors[port];
}

QStringList TrikBrick::motorPorts(trikControl::MotorInterface::Type type) const
{
	Q_UNUSED(type)
//	QLOG_INFO() << "Motor type is ignored";
	return mMotors.keys();
}

QStringList TrikBrick::sensorPorts(trikControl::SensorInterface::Type type) const
{
	Q_UNUSED(type)
//	QLOG_INFO() << "Sensor type is ignored";
	return mMotors.keys();
}

QStringList TrikBrick::encoderPorts() const {
	return mEncoders.keys();
}

trikControl::VectorSensorInterface *TrikBrick::accelerometer() {
	using namespace kitBase::robotModel;
	if (mAccelerometer.isNull()) {
		auto a = RobotModelUtils::findDevice<robotParts::AccelerometerSensor>(*mTwoDRobotModel
				, "AccelerometerPort");
		if (a == nullptr) {
			emit error(tr("No configured accelerometer"));
			return nullptr;
		}
		mAccelerometer.reset(new TrikAccelerometerAdapter(a));
	}

	return mAccelerometer.data();
}

trikControl::GyroSensorInterface *TrikBrick::gyroscope() {
	using namespace kitBase::robotModel;
	if (mGyroscope.isNull()) {
		auto a = RobotModelUtils::findDevice<robotParts::GyroscopeSensor>(*mTwoDRobotModel
				, "GyroscopePort");
		if (a == nullptr) {
			emit error(tr("No configured gyroscope"));
			return nullptr;
		}
		mGyroscope.reset(new TrikGyroscopeAdapter(a, mTwoDRobotModel));
	}

	return mGyroscope.data();
}

trikControl::LineSensorInterface *TrikBrick::lineSensor(const QString &port) {
	using namespace trik::robotModel::parts;
	using namespace kitBase::robotModel;
	if (port == "video0") {
		return lineSensor("LineSensorPort"); // seems to be the case for 2d model
	}

	if (!mLineSensors.contains(port)) {
		TrikLineSensor * sens =
				RobotModelUtils::findDevice<TrikLineSensor>(*mTwoDRobotModel, port);
		if (sens == nullptr) {
			emit error(tr("No configured LineSensor on port: %1").arg(port));
			return nullptr;
		}
		mLineSensors[port] = new TrikLineSensorAdapter(sens);
	}

	return mLineSensors[port];
}

trikControl::EncoderInterface *TrikBrick::encoder(const QString &port) {
	using namespace kitBase::robotModel;
	if (!mEncoders.contains(port)) {
		robotParts::EncoderSensor * enc =
				RobotModelUtils::findDevice<robotParts::EncoderSensor>(*mTwoDRobotModel, port);
		if (enc == nullptr) {
			emit error(tr("No configured encoder on port: %1").arg(port));
			return nullptr;
		}

		mEncoders[port] = new TrikEncoderAdapter(enc->port(), mTwoDRobotModel->engine());
	}

	return mEncoders[port];
}

trikControl::DisplayInterface *TrikBrick::display()
{
//	trik::robotModel::parts::TrikDisplay * const display =
//			kitBase::robotModel::RobotModelUtils::findDevice<trik::robotModel::parts::TrikDisplay>(*mTwoDRobotModel
//					, "DisplayPort");
//	if (display) {
//		bool res = QMetaObject::invokeMethod(display,
//		"drawSmile",
//		Qt::QueuedConnection, // connection type, auto?
//		Q_ARG(bool, false));
//		//display->drawSmile(false);
//		printf(res ? "true" : "false");
//	}
//	return nullptr;
	return &mDisplay;
}

trikControl::LedInterface *TrikBrick::led() {
	using namespace trik::robotModel::parts;
	using namespace kitBase::robotModel;
	if (mLed.isNull()) {
		auto l = RobotModelUtils::findDevice<TrikLed>(*mTwoDRobotModel, "LedPort");
		if (l == nullptr) {
			emit error(tr("No configured led"));
			return nullptr;
		}
		mLed.reset(new TrikLedAdapter(l));
	}

	return mLed.data();
}

int TrikBrick::random(int from, int to)
{
	using namespace kitBase::robotModel;
	auto r = RobotModelUtils::findDevice<robotParts::Random>(*mTwoDRobotModel, "RandomPort");
	// maybe store it later, like the others
	if (!r) {
		emit error(tr("No cofigured random device"));
		return -1;
	}

	return r->random(from, to);
}

void TrikBrick::wait(int milliseconds)
{
	auto timeline = dynamic_cast<twoDModel::model::Timeline *> (&mTwoDRobotModel->timeline());

	if (timeline->isStarted()) {
		QSharedPointer<utils::AbstractTimer> t(timeline->produceTimer());
		QTimer abortTimer;
		QMetaObject::Connection abortConnection;
		QSharedPointer<QEventLoop> loop (new QEventLoop());

		auto mainHandler = [=]() {
			disconnect(abortConnection);
			disconnect(this, &TrikBrick::stopWaiting, nullptr, nullptr);
			disconnect(timeline, &twoDModel::model::Timeline::beforeStop, nullptr, nullptr);
			disconnect(t.data(), &utils::AbstractTimer::timeout, nullptr, nullptr);
			loop->quit();
		};

		auto abortHandler = [=]() {
			if (!timeline->isStarted()) {
				mainHandler();
			}
		};

		connect(t.data(), &utils::AbstractTimer::timeout, mainHandler);
		connect(this, &TrikBrick::stopWaiting, mainHandler);

		// timers that are produced by produceTimer() doesn't use stop singal
		// be careful, one who use just utils::AbstractTimer can stuck
		connect(timeline, &twoDModel::model::Timeline::beforeStop, this, mainHandler);
		abortConnection = connect(&abortTimer, &QTimer::timeout, this, abortHandler);

		// because timer is depends on twoDModel::model::Timeline
		if (timeline->isStarted()) {
			t->start(milliseconds);
			abortTimer.start(10);
			loop->exec();
		} else {
			mainHandler();
		}
	}
}

quint64 TrikBrick::time() const
{
	return mTwoDRobotModel->timeline().timestamp();
}

QStringList TrikBrick::readAll(const QString &path)
{
	if (mIsExcerciseMode) {
		return mInputs;
	}
	//if (mCurrentDir) todo: check that the current working dir is a save dir
	QFileInfo normalizedPath(mCurrentDir.absoluteFilePath(path)); // absoluteDir?
	QString file = normalizedPath.filePath();
	QFile in(file);
	if (!in.open(QIODevice::ReadOnly | QIODevice::Text)) {
		emit error(tr("Trying to read from file %1 failed").arg(file));
		return {};
	}

	QStringList result;

	while (!in.atEnd()) {
		const auto line = in.readLine();
		result << QString::fromUtf8(line);
	}

	return result;
}

utils::AbstractTimer *TrikBrick::timer(int milliseconds)
{
	utils::AbstractTimer *result = mTwoDRobotModel->timeline().produceTimer();
	mTimers.append(result);
	result->setRepeatable(true); // seems to be the case
	result->start(milliseconds);
	return result;
}

<|endoftext|>