<commit_before>#include "Globals.h"
#include "Application.h"
#include "PhysBody3D.h"
#include "ModuleCamera3D.h"
#include "ModulePlayer.h"
#include "CameraFrustrum.h"

ModuleCamera3D::ModuleCamera3D(bool start_enabled) : Module(start_enabled)
{
	CalculateViewMatrix();
	zoom = 20;
	X = vec3(1.0f, 0.0f, 0.0f);
	Y = vec3(0.0f, 1.0f, 0.0f);
	Z = vec3(0.0f, 0.0f, 1.0f);

	Position = vec3(2.0f, 2.0f, 5.0f);
	Reference = vec3(0.0f, 0.0f, 0.0f);
}

ModuleCamera3D::~ModuleCamera3D()
{}

// -----------------------------------------------------------------
bool ModuleCamera3D::Start()
{
	LOG("Setting up the camera");
	bool ret = true;
	CamComp = new CameraComponent(App->scene_intro->root_gameobject,true);
	//Position = Reference + zoom * Z;

	return ret;
}

// -----------------------------------------------------------------
bool ModuleCamera3D::CleanUp()
{
	LOG("Cleaning camera");

	return true;
}
bool Gui_Engine_Modules(float dt) {

	return true;
}
// -----------------------------------------------------------------
update_status ModuleCamera3D::Update(float dt)
{
	// Implement a debug camera with keys and mouse
	// Now we can make this movememnt frame rate independant!
	vec3 newPos(0, 0, 0);
	float speed = 4.0f * dt;
	float wheel_direction = (float)App->input->GetMouseZ();

	if (wheel_direction > 0) Position += ((Reference-Position) * 0.1f);
	if (wheel_direction < 0) Position -= ((Reference - Position) * 0.1f);

	if (App->input->GetMouseButton(SDL_BUTTON_RIGHT) == KEY_DOWN && App->gui->n4 == false)
	{
		Can_Move_Camera = !Can_Move_Camera;
	}

	if (Can_Move_Camera) {
		if (App->input->GetKey(SDL_SCANCODE_R) == KEY_REPEAT) newPos.y += speed;
		if (App->input->GetKey(SDL_SCANCODE_F) == KEY_REPEAT) newPos.y -= speed;
		if (App->input->GetKey(SDL_SCANCODE_A) == KEY_REPEAT) newPos -= X * speed;
		if (App->input->GetKey(SDL_SCANCODE_D) == KEY_REPEAT) newPos += X * speed;
		if (App->input->GetKey(SDL_SCANCODE_S) == KEY_REPEAT) newPos += Z * speed;
		if (App->input->GetKey(SDL_SCANCODE_W) == KEY_REPEAT) newPos -= Z * speed;

		Move(newPos);
	}

			if ((App->input->GetMouseButton(SDL_BUTTON_LEFT) == KEY_REPEAT && App->input->GetKey(SDL_SCANCODE_LALT)== KEY_REPEAT && App->gui->n4 == false) || (App->input->GetMouseButton(SDL_BUTTON_LEFT) == KEY_REPEAT && Can_Move_Camera==true))
			{
				int dx = -App->input->GetMouseXMotion();
				int dy = -App->input->GetMouseYMotion();

				float Sensitivity = 0.25f;

				Position -= Reference;

				if (dx != 0)
				{
					float DeltaX = (float)dx * Sensitivity;

					X = rotate(X, DeltaX, vec3(0.0f, 1.0f, 0.0f));
					Y = rotate(Y, DeltaX, vec3(0.0f, 1.0f, 0.0f));
					Z = rotate(Z, DeltaX, vec3(0.0f, 1.0f, 0.0f));
				}

				if (dy != 0)
				{
					float DeltaY = (float)dy * Sensitivity;

					Y = rotate(Y, DeltaY, X);
					Z = rotate(Z, DeltaY, X);

					if (Y.y < 0.0f)
					{
						Z = vec3(0.0f, Z.y > 0.0f ? 1.0f : -1.0f, 0.0f);
						Y = cross(Z, X);
					}
				}

				Position = Reference + Z * length(Position);
			}

	// Recalculate matrix -------------
	//CalculateViewMatrix();

	return UPDATE_CONTINUE;
}

// -----------------------------------------------------------------
void ModuleCamera3D::Look(const vec3 &Position, const vec3 &Reference, bool RotateAroundReference)
{
	this->Position = Position;
	this->Reference = Reference;

	Z = normalize(Position - Reference);
	X = normalize(cross(vec3(0.0f, 1.0f, 0.0f), Z));
	Y = cross(Z, X);

	if (!RotateAroundReference)
	{
		this->Reference = this->Position;
		this->Position += Z * 0.05f;
	}

	CalculateViewMatrix();
}

// -----------------------------------------------------------------
void ModuleCamera3D::LookAt(const vec3 &Spot)
{
	Reference = Spot;

	Z = normalize(Position - Reference);
	X = normalize(cross(vec3(0.0f, 1.0f, 0.0f), Z));
	Y = cross(Z, X);

	CalculateViewMatrix();
}

// -----------------------------------------------------------------
void ModuleCamera3D::Move(const vec3 &Movement)
{
	Position += Movement;
	Reference += Movement;

	CalculateViewMatrix();
}

// -----------------------------------------------------------------
float* ModuleCamera3D::GetViewMatrix()
{
	return &ViewMatrix;
}

// -----------------------------------------------------------------
void ModuleCamera3D::CalculateViewMatrix()
{
	ViewMatrix = mat4x4(X.x, Y.x, Z.x, 0.0f, X.y, Y.y, Z.y, 0.0f, X.z, Y.z, Z.z, 0.0f, -dot(X, Position), -dot(Y, Position), -dot(Z, Position), 1.0f);
	ViewMatrixInverse = inverse(ViewMatrix);
}

void ModuleCamera3D::CameraRecenter()
{
	CameraCenter(LCenter);
}

void ModuleCamera3D::CameraCenter(AABB* mesh)
{
	if (mesh == nullptr)
	{
		Reference = vec3(0.0f, 0.0f, 0.0f);
	}
	else
	{
		vec centre = mesh->CenterPoint();
		Reference = vec3(centre.x, centre.y, centre.z);
		LCenter = mesh;
		Z = normalize(Position - Reference);

		//resize
		vec difference = mesh->maxPoint - mesh->minPoint;
		float wide = difference.Length() + 2.0f; //This magic number is just to have some frame around geometry
		float FOVdistance = (wide * 0.5f) / tan(60.0f * 0.5f * DEGTORAD);
		Position = Z * FOVdistance;
	}

	LookAt(Reference);
}
<commit_msg>Failed to change camera to component, reverted all the changes<commit_after>#include "Globals.h"
#include "Application.h"
#include "PhysBody3D.h"
#include "ModuleCamera3D.h"
#include "ModulePlayer.h"
#include "CameraFrustrum.h"

ModuleCamera3D::ModuleCamera3D(bool start_enabled) : Module(start_enabled)
{
	CalculateViewMatrix();
	zoom = 20;
	X = vec3(1.0f, 0.0f, 0.0f);
	Y = vec3(0.0f, 1.0f, 0.0f);
	Z = vec3(0.0f, 0.0f, 1.0f);

	Position = vec3(2.0f, 2.0f, 5.0f);
	Reference = vec3(0.0f, 0.0f, 0.0f);
}

ModuleCamera3D::~ModuleCamera3D()
{}

// -----------------------------------------------------------------
bool ModuleCamera3D::Start()
{
	LOG("Setting up the camera");
	bool ret = true;
	CamComp = new CameraComponent(App->scene_intro->root_gameobject,true);
	//Position = Reference + zoom * Z;

	return ret;
}

// -----------------------------------------------------------------
bool ModuleCamera3D::CleanUp()
{
	LOG("Cleaning camera");

	return true;
}
bool Gui_Engine_Modules(float dt) {

	return true;
}
// -----------------------------------------------------------------
update_status ModuleCamera3D::Update(float dt)
{
	// Implement a debug camera with keys and mouse
	// Now we can make this movememnt frame rate independant!
	vec3 newPos(0, 0, 0);
	float speed = 4.0f * dt;
	float wheel_direction = (float)App->input->GetMouseZ();

	if (wheel_direction > 0) Position += ((Reference-Position) * 0.1f);
	if (wheel_direction < 0) Position -= ((Reference - Position) * 0.1f);

	if (App->input->GetMouseButton(SDL_BUTTON_RIGHT) == KEY_DOWN && App->gui->n4 == false)
	{
		Can_Move_Camera = !Can_Move_Camera;
	}

	if (Can_Move_Camera) {
		if (App->input->GetKey(SDL_SCANCODE_R) == KEY_REPEAT) newPos.y += speed;
		if (App->input->GetKey(SDL_SCANCODE_F) == KEY_REPEAT) newPos.y -= speed;
		if (App->input->GetKey(SDL_SCANCODE_A) == KEY_REPEAT) newPos -= X * speed;
		if (App->input->GetKey(SDL_SCANCODE_D) == KEY_REPEAT) newPos += X * speed;
		if (App->input->GetKey(SDL_SCANCODE_S) == KEY_REPEAT) newPos += Z * speed;
		if (App->input->GetKey(SDL_SCANCODE_W) == KEY_REPEAT) newPos -= Z * speed;

		Move(newPos);
	}

			if ((App->input->GetMouseButton(SDL_BUTTON_LEFT) == KEY_REPEAT && App->input->GetKey(SDL_SCANCODE_LALT)== KEY_REPEAT && App->gui->n4 == false) || (App->input->GetMouseButton(SDL_BUTTON_LEFT) == KEY_REPEAT && Can_Move_Camera==true))
			{
				int dx = -App->input->GetMouseXMotion();
				int dy = -App->input->GetMouseYMotion();

				float Sensitivity = 0.25f;

				Position -= Reference;

				if (dx != 0)
				{
					float DeltaX = (float)dx * Sensitivity;

					X = rotate(X, DeltaX, vec3(0.0f, 1.0f, 0.0f));
					Y = rotate(Y, DeltaX, vec3(0.0f, 1.0f, 0.0f));
					Z = rotate(Z, DeltaX, vec3(0.0f, 1.0f, 0.0f));
				}

				if (dy != 0)
				{
					float DeltaY = (float)dy * Sensitivity;

					Y = rotate(Y, DeltaY, X);
					Z = rotate(Z, DeltaY, X);

					if (Y.y < 0.0f)
					{
						Z = vec3(0.0f, Z.y > 0.0f ? 1.0f : -1.0f, 0.0f);
						Y = cross(Z, X);
					}
				}

				Position = Reference + Z * length(Position);
			}

	// Recalculate matrix -------------
	CalculateViewMatrix();

	return UPDATE_CONTINUE;
}

// -----------------------------------------------------------------
void ModuleCamera3D::Look(const vec3 &Position, const vec3 &Reference, bool RotateAroundReference)
{
	this->Position = Position;
	this->Reference = Reference;

	Z = normalize(Position - Reference);
	X = normalize(cross(vec3(0.0f, 1.0f, 0.0f), Z));
	Y = cross(Z, X);

	if (!RotateAroundReference)
	{
		this->Reference = this->Position;
		this->Position += Z * 0.05f;
	}

	CalculateViewMatrix();
}

// -----------------------------------------------------------------
void ModuleCamera3D::LookAt(const vec3 &Spot)
{
	Reference = Spot;

	Z = normalize(Position - Reference);
	X = normalize(cross(vec3(0.0f, 1.0f, 0.0f), Z));
	Y = cross(Z, X);

	CalculateViewMatrix();
}

// -----------------------------------------------------------------
void ModuleCamera3D::Move(const vec3 &Movement)
{
	Position += Movement;
	Reference += Movement;

	CalculateViewMatrix();
}

// -----------------------------------------------------------------
float* ModuleCamera3D::GetViewMatrix()
{
	return &ViewMatrix;
}

// -----------------------------------------------------------------
void ModuleCamera3D::CalculateViewMatrix()
{
	ViewMatrix = mat4x4(X.x, Y.x, Z.x, 0.0f, X.y, Y.y, Z.y, 0.0f, X.z, Y.z, Z.z, 0.0f, -dot(X, Position), -dot(Y, Position), -dot(Z, Position), 1.0f);
	ViewMatrixInverse = inverse(ViewMatrix);
}

void ModuleCamera3D::CameraRecenter()
{
	CameraCenter(LCenter);
}

void ModuleCamera3D::CameraCenter(AABB* mesh)
{
	if (mesh == nullptr)
	{
		Reference = vec3(0.0f, 0.0f, 0.0f);
	}
	else
	{
		vec centre = mesh->CenterPoint();
		Reference = vec3(centre.x, centre.y, centre.z);
		LCenter = mesh;
		Z = normalize(Position - Reference);

		//resize
		vec difference = mesh->maxPoint - mesh->minPoint;
		float wide = difference.Length() + 2.0f; //This magic number is just to have some frame around geometry
		float FOVdistance = (wide * 0.5f) / tan(60.0f * 0.5f * DEGTORAD);
		Position = Z * FOVdistance;
	}

	LookAt(Reference);
}
<|endoftext|>


<commit_before>#include "graphics/sprite/animation.h"
#include "graphics/sprite/animation_player.h"
#include "graphics/sprite/sprite.h"
#include <gsl/gsl_assert>
#include "resources/resources.h"

using namespace Halley;

AnimationPlayer::AnimationPlayer(std::shared_ptr<const Animation> animation, const String& sequence, const String& direction)
{
	setAnimation(animation, sequence, direction);
}

AnimationPlayer& AnimationPlayer::playOnce(const String& sequence, const std::optional<String>& nextLoopingSequence)
{
	updateIfNeeded();

	curSeq = nullptr;
	setSequence(sequence);
	seqLooping = false;

	nextSequence = nextLoopingSequence;

	return *this;
}

AnimationPlayer& AnimationPlayer::setAnimation(std::shared_ptr<const Animation> v, const String& sequence, const String& direction)
{
	if (animation != v) {
		animation = v;
		if (animation) {
			observer.startObserving(*animation);
		} else {
			observer.stopObserving();
		}
		curDir = nullptr;
		curSeq = nullptr;
		dirId = -1;
	}

	updateIfNeeded();

	if (animation) {
		setSequence(sequence);
		setDirection(direction);
	}
	return *this;
}

AnimationPlayer& AnimationPlayer::setSequence(const String& _sequence)
{
	curSeqName = _sequence; // DO NOT use _sequence after this, it can be a reference to nextSequence, which is changed on the next line
	nextSequence = {};
	updateIfNeeded();

	if (animation && (!curSeq || curSeq->getName() != curSeqName)) {
		curSeqTime = 0;
		curFrameTime = 0;
		curFrame = 0;
		curFrameLen = 0;
		curLoopCount = 0;
		curSeq = &animation->getSequence(curSeqName);

		seqLen = curSeq->numFrames();
		seqLooping = curSeq->isLooping();
		seqNoFlip = curSeq->isNoFlip();

		dirty = true;

		onSequenceStarted();
	}
	return *this;
}

AnimationPlayer& AnimationPlayer::setDirection(int direction)
{
	updateIfNeeded();

	if (animation && dirId != direction) {
		auto newDir = &animation->getDirection(direction);
		if (curDir != newDir) {
			curDir = newDir;
			dirFlip = curDir->shouldFlip();
			dirId = curDir->getId();
			dirty = true;
		}
	}
	return *this;
}

AnimationPlayer& AnimationPlayer::setDirection(const String& direction)
{
	curDirName = direction;
	updateIfNeeded();

	if (animation && (!curDir || curDir->getName() != direction)) {
		auto newDir = &animation->getDirection(direction);
		if (curDir != newDir) {
			curDir = newDir;
			dirFlip = curDir->shouldFlip();
			dirId = curDir->getId();
			dirty = true;
		}
	}
	return *this;
}

bool AnimationPlayer::trySetSequence(const String& sequence)
{
	updateIfNeeded();
	if (animation && animation->hasSequence(sequence)) {
		setSequence(sequence);
		return true;
	}
	return false;
}

AnimationPlayer& AnimationPlayer::setApplyPivot(bool apply)
{
	applyPivot = apply;
	return *this;
}

bool AnimationPlayer::isApplyingPivot() const
{
	return applyPivot;
}

void AnimationPlayer::update(Time time)
{
	updateIfNeeded();

	if (!animation) {
		return;
	}

	if (dirty) {
		resolveSprite();
		dirty = false;
	}

	const int prevFrame = curFrame;

	curSeqTime += time * playbackSpeed;
	curFrameTime += time * playbackSpeed;

	// Next frame time!
	if (curFrameTime >= curFrameLen) {
		for (int i = 0; i < 5 && curFrameTime >= curFrameLen; ++i) {
			curFrame++;
			curFrameTime -= curFrameLen;

			if (curFrame >= int(seqLen)) {
				if (seqLooping) {
					curFrame = 0;
					curSeqTime = curFrameTime;
					curLoopCount++;
				} else {
					curFrame = int(seqLen - 1);
					onSequenceDone();
				}
			}
		}
	}

	if (curFrame != prevFrame) {
		resolveSprite();
	}
}

void AnimationPlayer::updateSprite(Sprite& sprite) const
{
	if (animation && hasUpdate) {
		if (materialOverride) {
			sprite.setMaterial(materialOverride, true);
		} else {
			sprite.setMaterial(animation->getMaterial(), true);
		}

		sprite.setSprite(*spriteData, false);

		if (applyPivot && spriteData->pivot.isValid()) {
			auto sz = sprite.getSize();
			if (std::abs(sz.x) < 0.00001f) {
				sz.x = 1;
			}
			if (std::abs(sz.y) < 0.00001f) {
				sz.y = 1;
			}
			sprite.setPivot(spriteData->pivot + offsetPivot / sz);
		}
		sprite.setFlip(dirFlip && !seqNoFlip);
		if (visibleOverride) {
			sprite.setVisible(visibleOverride.value());
		}

		hasUpdate = false;
	}
}

AnimationPlayer& AnimationPlayer::setMaterialOverride(std::shared_ptr<Material> material)
{
	materialOverride = std::move(material);
	return *this;
}

std::shared_ptr<Material> AnimationPlayer::getMaterialOverride() const
{
	return materialOverride;
}

std::shared_ptr<const Material> AnimationPlayer::getMaterial() const
{
	return animation->getMaterial();
}

bool AnimationPlayer::isPlaying() const
{
	return playing;
}

String AnimationPlayer::getCurrentSequenceName() const
{
	return curSeq ? curSeq->getName() : "";
}

Time AnimationPlayer::getCurrentSequenceTime() const
{
	return curSeqTime;
}

int AnimationPlayer::getCurrentSequenceFrame() const
{
	return curFrame;
}

int AnimationPlayer::getCurrentSequenceLoopCount() const
{
	return curLoopCount;
}

String AnimationPlayer::getCurrentDirectionName() const
{
	return curDir ? curDir->getName() : "default";
}

AnimationPlayer& AnimationPlayer::setPlaybackSpeed(float value)
{
	playbackSpeed = value;
	return *this;
}

float AnimationPlayer::getPlaybackSpeed() const
{
	return playbackSpeed;
}

const Animation& AnimationPlayer::getAnimation() const
{
	return *animation;
}

std::shared_ptr<const Animation> AnimationPlayer::getAnimationPtr() const
{
	return animation;
}

bool AnimationPlayer::hasAnimation() const
{
	return static_cast<bool>(animation);
}

AnimationPlayer& AnimationPlayer::setOffsetPivot(Vector2f offset)
{
	offsetPivot = offset;
	hasUpdate = true;
	return *this;
}

void AnimationPlayer::syncWith(const AnimationPlayer& masterAnimator, bool hideIfNotSynchronized)
{
	setSequence(masterAnimator.getCurrentSequenceName());
	setDirection(masterAnimator.getCurrentDirectionName());
	visibleOverride = !hideIfNotSynchronized || getCurrentSequenceName() == masterAnimator.getCurrentSequenceName();
	curFrame = clamp(masterAnimator.curFrame, 0, static_cast<int>(curSeq->numFrames()) - 1);
	curFrameTime = masterAnimator.curFrameTime;

	resolveSprite();
}

void AnimationPlayer::stepFrames(int amount)
{
	if (amount != 0) {
		curFrame = modulo(curFrame + amount, static_cast<int>(seqLen));
		curFrameTime = 0;

		resolveSprite();
	}
}

void AnimationPlayer::resolveSprite()
{
	updateIfNeeded();

	const auto& frame = curSeq->getFrame(curFrame);
	curFrameLen = std::max(1, frame.getDuration()) * 0.001; // 1ms minimum
	spriteData = &frame.getSprite(dirId);
	hasUpdate = true;
}

void AnimationPlayer::onSequenceStarted()
{
	playing = true;
}

void AnimationPlayer::onSequenceDone()
{
	if (nextSequence) {
		setSequence(nextSequence.value());
	} else {
		playing = false;
	}
}

void AnimationPlayer::updateIfNeeded()
{
	if (observer.needsUpdate()) {
		observer.update();
		dirId = -1;
		curDir = nullptr;
		curSeq = nullptr;
		setSequence(curSeqName);
		setDirection(curDirName);
	}
}

ConfigNode ConfigNodeSerializer<AnimationPlayer>::serialize(const AnimationPlayer& player, ConfigNodeSerializationContext& context)
{
	ConfigNode result = ConfigNode::MapType();
	result["animation"] = player.hasAnimation() ? player.getAnimation().getAssetId() : "";
	result["sequence"] = player.getCurrentSequenceName();
	result["direction"] = player.getCurrentDirectionName();
	result["applyPivot"] = player.isApplyingPivot();
	result["playbackSpeed"] = player.getPlaybackSpeed();
	return result;
}

AnimationPlayer ConfigNodeSerializer<AnimationPlayer>::deserialize(ConfigNodeSerializationContext& context, const ConfigNode& node)
{
	auto animName = node["animation"].asString("");
	auto anim = animName.isEmpty() ? std::shared_ptr<Animation>() : context.resources->get<Animation>(animName);

	auto player = AnimationPlayer(anim, node["sequence"].asString("default"), node["direction"].asString("default"));
	player.setApplyPivot(node["applyPivot"].asBool(true));
	player.setPlaybackSpeed(node["playbackSpeed"].asFloat(1.0f));
	return player;
}
<commit_msg>Fix edge cases around syncing AnimationPlayers<commit_after>#include "graphics/sprite/animation.h"
#include "graphics/sprite/animation_player.h"
#include "graphics/sprite/sprite.h"
#include <gsl/gsl_assert>
#include "resources/resources.h"

using namespace Halley;

AnimationPlayer::AnimationPlayer(std::shared_ptr<const Animation> animation, const String& sequence, const String& direction)
{
	setAnimation(animation, sequence, direction);
}

AnimationPlayer& AnimationPlayer::playOnce(const String& sequence, const std::optional<String>& nextLoopingSequence)
{
	updateIfNeeded();

	curSeq = nullptr;
	setSequence(sequence);
	seqLooping = false;

	nextSequence = nextLoopingSequence;

	return *this;
}

AnimationPlayer& AnimationPlayer::setAnimation(std::shared_ptr<const Animation> v, const String& sequence, const String& direction)
{
	if (animation != v) {
		animation = v;
		if (animation) {
			observer.startObserving(*animation);
		} else {
			observer.stopObserving();
		}
		curDir = nullptr;
		curSeq = nullptr;
		dirId = -1;
	}

	updateIfNeeded();

	if (animation) {
		setSequence(sequence);
		setDirection(direction);
	}
	return *this;
}

AnimationPlayer& AnimationPlayer::setSequence(const String& _sequence)
{
	curSeqName = _sequence; // DO NOT use _sequence after this, it can be a reference to nextSequence, which is changed on the next line
	nextSequence = {};
	updateIfNeeded();

	if (animation && (!curSeq || curSeq->getName() != curSeqName)) {
		curSeqTime = 0;
		curFrameTime = 0;
		curFrame = 0;
		curFrameLen = 0;
		curLoopCount = 0;
		curSeq = &animation->getSequence(curSeqName);

		seqLen = curSeq->numFrames();
		seqLooping = curSeq->isLooping();
		seqNoFlip = curSeq->isNoFlip();

		dirty = true;

		onSequenceStarted();
	}
	return *this;
}

AnimationPlayer& AnimationPlayer::setDirection(int direction)
{
	updateIfNeeded();

	if (animation && dirId != direction) {
		auto newDir = &animation->getDirection(direction);
		if (curDir != newDir) {
			curDir = newDir;
			dirFlip = curDir->shouldFlip();
			dirId = curDir->getId();
			dirty = true;
		}
	}
	return *this;
}

AnimationPlayer& AnimationPlayer::setDirection(const String& direction)
{
	curDirName = direction;
	updateIfNeeded();

	if (animation && (!curDir || curDir->getName() != direction)) {
		auto newDir = &animation->getDirection(direction);
		if (curDir != newDir) {
			curDir = newDir;
			dirFlip = curDir->shouldFlip();
			dirId = curDir->getId();
			dirty = true;
		}
	}
	return *this;
}

bool AnimationPlayer::trySetSequence(const String& sequence)
{
	updateIfNeeded();
	if (animation && animation->hasSequence(sequence)) {
		setSequence(sequence);
		return true;
	}
	return false;
}

AnimationPlayer& AnimationPlayer::setApplyPivot(bool apply)
{
	applyPivot = apply;
	return *this;
}

bool AnimationPlayer::isApplyingPivot() const
{
	return applyPivot;
}

void AnimationPlayer::update(Time time)
{
	updateIfNeeded();

	if (!animation) {
		return;
	}

	if (dirty) {
		resolveSprite();
		dirty = false;
	}

	const int prevFrame = curFrame;

	curSeqTime += time * playbackSpeed;
	curFrameTime += time * playbackSpeed;

	// Next frame time!
	if (curFrameTime >= curFrameLen) {
		for (int i = 0; i < 5 && curFrameTime >= curFrameLen; ++i) {
			curFrame++;
			curFrameTime -= curFrameLen;

			if (curFrame >= int(seqLen)) {
				if (seqLooping) {
					curFrame = 0;
					curSeqTime = curFrameTime;
					curLoopCount++;
				} else {
					curFrame = int(seqLen - 1);
					onSequenceDone();
				}
			}
		}
	}

	if (curFrame != prevFrame) {
		resolveSprite();
	}
}

void AnimationPlayer::updateSprite(Sprite& sprite) const
{
	if (animation && hasUpdate) {
		if (materialOverride) {
			sprite.setMaterial(materialOverride, true);
		} else {
			sprite.setMaterial(animation->getMaterial(), true);
		}

		sprite.setSprite(*spriteData, false);

		if (applyPivot && spriteData->pivot.isValid()) {
			auto sz = sprite.getSize();
			if (std::abs(sz.x) < 0.00001f) {
				sz.x = 1;
			}
			if (std::abs(sz.y) < 0.00001f) {
				sz.y = 1;
			}
			sprite.setPivot(spriteData->pivot + offsetPivot / sz);
		}
		sprite.setFlip(dirFlip && !seqNoFlip);
		if (visibleOverride) {
			sprite.setVisible(visibleOverride.value());
		}

		hasUpdate = false;
	}
}

AnimationPlayer& AnimationPlayer::setMaterialOverride(std::shared_ptr<Material> material)
{
	materialOverride = std::move(material);
	return *this;
}

std::shared_ptr<Material> AnimationPlayer::getMaterialOverride() const
{
	return materialOverride;
}

std::shared_ptr<const Material> AnimationPlayer::getMaterial() const
{
	return animation->getMaterial();
}

bool AnimationPlayer::isPlaying() const
{
	return playing;
}

String AnimationPlayer::getCurrentSequenceName() const
{
	return curSeq ? curSeq->getName() : "";
}

Time AnimationPlayer::getCurrentSequenceTime() const
{
	return curSeqTime;
}

int AnimationPlayer::getCurrentSequenceFrame() const
{
	return curFrame;
}

int AnimationPlayer::getCurrentSequenceLoopCount() const
{
	return curLoopCount;
}

String AnimationPlayer::getCurrentDirectionName() const
{
	return curDir ? curDir->getName() : "default";
}

AnimationPlayer& AnimationPlayer::setPlaybackSpeed(float value)
{
	playbackSpeed = value;
	return *this;
}

float AnimationPlayer::getPlaybackSpeed() const
{
	return playbackSpeed;
}

const Animation& AnimationPlayer::getAnimation() const
{
	return *animation;
}

std::shared_ptr<const Animation> AnimationPlayer::getAnimationPtr() const
{
	return animation;
}

bool AnimationPlayer::hasAnimation() const
{
	return static_cast<bool>(animation);
}

AnimationPlayer& AnimationPlayer::setOffsetPivot(Vector2f offset)
{
	offsetPivot = offset;
	hasUpdate = true;
	return *this;
}

void AnimationPlayer::syncWith(const AnimationPlayer& masterAnimator, bool hideIfNotSynchronized)
{
	setSequence(masterAnimator.getCurrentSequenceName());
	setDirection(masterAnimator.getCurrentDirectionName());
	visibleOverride = !hideIfNotSynchronized || getCurrentSequenceName() == masterAnimator.getCurrentSequenceName();
	curFrame = clamp(masterAnimator.curFrame, 0, curSeq ? static_cast<int>(curSeq->numFrames()) - 1 : 0);
	curFrameTime = masterAnimator.curFrameTime;

	resolveSprite();
}

void AnimationPlayer::stepFrames(int amount)
{
	if (amount != 0) {
		curFrame = modulo(curFrame + amount, static_cast<int>(seqLen));
		curFrameTime = 0;

		resolveSprite();
	}
}

void AnimationPlayer::resolveSprite()
{
	updateIfNeeded();

	if (curSeq) {
		const auto& frame = curSeq->getFrame(curFrame);
		curFrameLen = std::max(1, frame.getDuration()) * 0.001; // 1ms minimum
		spriteData = &frame.getSprite(dirId);
		hasUpdate = true;
	}
}

void AnimationPlayer::onSequenceStarted()
{
	playing = true;
}

void AnimationPlayer::onSequenceDone()
{
	if (nextSequence) {
		setSequence(nextSequence.value());
	} else {
		playing = false;
	}
}

void AnimationPlayer::updateIfNeeded()
{
	if (observer.needsUpdate()) {
		observer.update();
		dirId = -1;
		curDir = nullptr;
		curSeq = nullptr;
		setSequence(curSeqName);
		setDirection(curDirName);
	}
}

ConfigNode ConfigNodeSerializer<AnimationPlayer>::serialize(const AnimationPlayer& player, ConfigNodeSerializationContext& context)
{
	ConfigNode result = ConfigNode::MapType();
	result["animation"] = player.hasAnimation() ? player.getAnimation().getAssetId() : "";
	result["sequence"] = player.getCurrentSequenceName();
	result["direction"] = player.getCurrentDirectionName();
	result["applyPivot"] = player.isApplyingPivot();
	result["playbackSpeed"] = player.getPlaybackSpeed();
	return result;
}

AnimationPlayer ConfigNodeSerializer<AnimationPlayer>::deserialize(ConfigNodeSerializationContext& context, const ConfigNode& node)
{
	auto animName = node["animation"].asString("");
	auto anim = animName.isEmpty() ? std::shared_ptr<Animation>() : context.resources->get<Animation>(animName);

	auto player = AnimationPlayer(anim, node["sequence"].asString("default"), node["direction"].asString("default"));
	player.setApplyPivot(node["applyPivot"].asBool(true));
	player.setPlaybackSpeed(node["playbackSpeed"].asFloat(1.0f));
	return player;
}
<|endoftext|>


<commit_before>#include "mbed.h"
#include "greentea-client/test_env.h"
#include "unity.h"
#include "utest.h"

#include "I2CEEBlockDevice.h"
#include <stdlib.h>

using namespace utest::v1;

#define TEST_PINS D14, D15
#define TEST_ADDR 0xa0
#define TEST_SIZE 32*1024
#define TEST_BLOCK_SIZE 64
#define TEST_FREQ 400000
#define TEST_BLOCK_COUNT 10
#define TEST_ERROR_MASK 16

const struct {
    const char *name;
    bd_size_t (BlockDevice::*method)() const;
} ATTRS[] = {
    {"read size",    &BlockDevice::get_read_size},
    {"program size", &BlockDevice::get_program_size},
    {"erase size",   &BlockDevice::get_erase_size},
    {"total size",   &BlockDevice::size},
};

void test_read_write() {
    I2CEEBlockDevice bd(TEST_PINS, TEST_ADDR,
        TEST_SIZE, TEST_BLOCK_SIZE, TEST_FREQ);

    int err = bd.init();
    TEST_ASSERT_EQUAL(0, err);

    for (unsigned a = 0; a < sizeof(ATTRS)/sizeof(ATTRS[0]); a++) {
        static const char *prefixes[] = {"", "k", "M", "G"};
        for (int i = 3; i >= 0; i--) {
            bd_size_t size = (bd.*ATTRS[a].method)();
            if (size >= (1ULL << 10*i)) {
                printf("%s: %llu%sbytes (%llubytes)\n",
                    ATTRS[a].name, size >> 10*i, prefixes[i], size);
                break;
            }
        }
    }

    bd_size_t block_size = bd.get_erase_size();
    uint8_t *write_block = new uint8_t[block_size];
    uint8_t *read_block = new uint8_t[block_size];
    uint8_t *error_mask = new uint8_t[TEST_ERROR_MASK];
    unsigned addrwidth = ceil(log(bd.size()-1) / log(16))+1;

    for (int b = 0; b < TEST_BLOCK_COUNT; b++) {
        // Find a random block
        bd_addr_t block = (rand()*block_size) % bd.size();

        // Use next random number as temporary seed to keep
        // the address progressing in the pseudorandom sequence
        unsigned seed = rand();

        // Fill with random sequence
        srand(seed);
        for (bd_size_t i = 0; i < block_size; i++) {
            write_block[i] = 0xff & rand();
        }

        // Write, sync, and read the block
        printf("test  %0*llx:%llu...\n", addrwidth, block, block_size);

        err = bd.erase(block, block_size);
        TEST_ASSERT_EQUAL(0, err);

        err = bd.program(write_block, block, block_size);
        TEST_ASSERT_EQUAL(0, err);

        printf("write %0*llx:%llu ", addrwidth, block, block_size);
        for (int i = 0; i < block_size && i < 16; i++) {
            printf("%02x", write_block[i]);
        }
        if (block_size > 16) {
            printf("...\n");
        }
        printf("\n");

        err = bd.read(read_block, block, block_size);
        TEST_ASSERT_EQUAL(0, err);

        printf("read  %0*llx:%llu ", addrwidth, block, block_size);
        for (int i = 0; i < block_size && i < 16; i++) {
            printf("%02x", read_block[i]);
        }
        if (block_size > 16) {
            printf("...");
        }
        printf("\n");

        // Find error mask for debugging
        memset(error_mask, 0, TEST_ERROR_MASK);
        bd_size_t error_scale = block_size / (TEST_ERROR_MASK*8);

        srand(seed);
        for (bd_size_t i = 0; i < TEST_ERROR_MASK*8; i++) {
            for (bd_size_t j = 0; j < error_scale; j++) {
                if ((0xff & rand()) != read_block[i*error_scale + j]) {
                    error_mask[i/8] |= 1 << (i%8);
                }
            }
        }

        printf("error %0*llx:%llu ", addrwidth, block, block_size);
        for (int i = 0; i < TEST_ERROR_MASK; i++) {
            printf("%02x", error_mask[i]);
        }
        printf("\n");

        // Check that the data was unmodified
        srand(seed);
        for (bd_size_t i = 0; i < block_size; i++) {
            TEST_ASSERT_EQUAL(0xff & rand(), read_block[i]);
        }
    }

    err = bd.deinit();
    TEST_ASSERT_EQUAL(0, err);
}

// Test setup
utest::v1::status_t test_setup(const size_t number_of_cases) {
    GREENTEA_SETUP(30, "default_auto");
    return verbose_test_setup_handler(number_of_cases);
}

Case cases[] = {
    Case("Testing read write random blocks", test_read_write),
};

Specification specification(test_setup, cases);

int main() {
    return !Harness::run(specification);
}
<commit_msg>Adding explicit type conversion to overloaded log function as ARMCC fails to compile<commit_after>#include "mbed.h"
#include "greentea-client/test_env.h"
#include "unity.h"
#include "utest.h"

#include "I2CEEBlockDevice.h"
#include <stdlib.h>

using namespace utest::v1;

#define TEST_PINS D14, D15
#define TEST_ADDR 0xa0
#define TEST_SIZE 32*1024
#define TEST_BLOCK_SIZE 64
#define TEST_FREQ 400000
#define TEST_BLOCK_COUNT 10
#define TEST_ERROR_MASK 16

const struct {
    const char *name;
    bd_size_t (BlockDevice::*method)() const;
} ATTRS[] = {
    {"read size",    &BlockDevice::get_read_size},
    {"program size", &BlockDevice::get_program_size},
    {"erase size",   &BlockDevice::get_erase_size},
    {"total size",   &BlockDevice::size},
};

void test_read_write() {
    I2CEEBlockDevice bd(TEST_PINS, TEST_ADDR,
        TEST_SIZE, TEST_BLOCK_SIZE, TEST_FREQ);

    int err = bd.init();
    TEST_ASSERT_EQUAL(0, err);

    for (unsigned a = 0; a < sizeof(ATTRS)/sizeof(ATTRS[0]); a++) {
        static const char *prefixes[] = {"", "k", "M", "G"};
        for (int i = 3; i >= 0; i--) {
            bd_size_t size = (bd.*ATTRS[a].method)();
            if (size >= (1ULL << 10*i)) {
                printf("%s: %llu%sbytes (%llubytes)\n",
                    ATTRS[a].name, size >> 10*i, prefixes[i], size);
                break;
            }
        }
    }

    bd_size_t block_size = bd.get_erase_size();
    uint8_t *write_block = new uint8_t[block_size];
    uint8_t *read_block = new uint8_t[block_size];
    uint8_t *error_mask = new uint8_t[TEST_ERROR_MASK];
    unsigned addrwidth = ceil(log(float(bd.size()-1)) / log(float(16)))+1;

    for (int b = 0; b < TEST_BLOCK_COUNT; b++) {
        // Find a random block
        bd_addr_t block = (rand()*block_size) % bd.size();

        // Use next random number as temporary seed to keep
        // the address progressing in the pseudorandom sequence
        unsigned seed = rand();

        // Fill with random sequence
        srand(seed);
        for (bd_size_t i = 0; i < block_size; i++) {
            write_block[i] = 0xff & rand();
        }

        // Write, sync, and read the block
        printf("test  %0*llx:%llu...\n", addrwidth, block, block_size);

        err = bd.erase(block, block_size);
        TEST_ASSERT_EQUAL(0, err);

        err = bd.program(write_block, block, block_size);
        TEST_ASSERT_EQUAL(0, err);

        printf("write %0*llx:%llu ", addrwidth, block, block_size);
        for (int i = 0; i < block_size && i < 16; i++) {
            printf("%02x", write_block[i]);
        }
        if (block_size > 16) {
            printf("...\n");
        }
        printf("\n");

        err = bd.read(read_block, block, block_size);
        TEST_ASSERT_EQUAL(0, err);

        printf("read  %0*llx:%llu ", addrwidth, block, block_size);
        for (int i = 0; i < block_size && i < 16; i++) {
            printf("%02x", read_block[i]);
        }
        if (block_size > 16) {
            printf("...");
        }
        printf("\n");

        // Find error mask for debugging
        memset(error_mask, 0, TEST_ERROR_MASK);
        bd_size_t error_scale = block_size / (TEST_ERROR_MASK*8);

        srand(seed);
        for (bd_size_t i = 0; i < TEST_ERROR_MASK*8; i++) {
            for (bd_size_t j = 0; j < error_scale; j++) {
                if ((0xff & rand()) != read_block[i*error_scale + j]) {
                    error_mask[i/8] |= 1 << (i%8);
                }
            }
        }

        printf("error %0*llx:%llu ", addrwidth, block, block_size);
        for (int i = 0; i < TEST_ERROR_MASK; i++) {
            printf("%02x", error_mask[i]);
        }
        printf("\n");

        // Check that the data was unmodified
        srand(seed);
        for (bd_size_t i = 0; i < block_size; i++) {
            TEST_ASSERT_EQUAL(0xff & rand(), read_block[i]);
        }
    }

    err = bd.deinit();
    TEST_ASSERT_EQUAL(0, err);
}

// Test setup
utest::v1::status_t test_setup(const size_t number_of_cases) {
    GREENTEA_SETUP(30, "default_auto");
    return verbose_test_setup_handler(number_of_cases);
}

Case cases[] = {
    Case("Testing read write random blocks", test_read_write),
};

Specification specification(test_setup, cases);

int main() {
    return !Harness::run(specification);
}
<|endoftext|>


<commit_before>#include <stdio.h>
#include <stdint.h>
#include <inttypes.h>
#include <ctype.h>
#include <unistd.h>

#include <opencv2/opencv.hpp>

#include <AprilTags/apriltag.h>
#include <AprilTags/common/image_u8.h>
#include <AprilTags/tag36h11.h>
#include <AprilTags/common/zarray.h>
#include <AprilTags/common/getopt.h>

#include <drc_utils/LcmWrapper.hpp>
#include <drc_utils/BotWrapper.hpp>
#include <lcm/lcm-cpp.hpp>

#include <lcmtypes/drc/robot_state_t.hpp>
#include <lcmtypes/bot_core/images_t.hpp>
#include <lcmtypes/bot_core/image_t.hpp>

#include <bot_core/camtrans.h>
#include <bot_param/param_util.h>

#include <vector>
#include <iostream>

#include <Eigen/Dense>

struct TagMatch {
    cv::Point2d p0, p1, p2, p3;
    Eigen::Matrix3d H;
};

class AprilTagDetector {
    public:
    AprilTagDetector(getopt_t *options) : getopt(options) {
        tf = tag36h11_create();
        tf->black_border = getopt_get_int(options, "border");
        td = apriltag_detector_create();
        apriltag_detector_add_family(td, tf);

        td->quad_decimate = getopt_get_double(getopt, "decimate");
        td->quad_sigma = getopt_get_double(getopt, "blur");
        td->nthreads = getopt_get_int(getopt, "threads");
        td->debug = getopt_get_bool(getopt, "debug");
        td->refine_edges = getopt_get_bool(getopt, "refine-edges");
        td->refine_decode = getopt_get_bool(getopt, "refine-decode");
        td->refine_pose = getopt_get_bool(getopt, "refine-pose");

        quiet = getopt_get_bool(getopt, "quiet");
    }

    ~AprilTagDetector() {

        apriltag_detector_destroy(td);
        tag36h11_destroy(tf);
    }

    std::vector<TagMatch> detectTags(image_u8_t *im) {

        const int hamm_hist_max = 10;

        int hamm_hist[hamm_hist_max];
        memset(hamm_hist, 0, sizeof(hamm_hist));
        //image_u8_t *im = image_u8_create_from_pnm(path);

        zarray_t *detections = apriltag_detector_detect(td, im);

        std::vector<TagMatch> tag_matches;
        for (int i = 0; i < zarray_size(detections); i++) {
            apriltag_detection_t *det;
            zarray_get(detections, i, &det);

            if (!quiet)
                printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n",
                       i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);

            for (int x = 0; x < 3 ; x++) {
                image_u8_draw_line(im, det->p[x][0], det->p[x][1], det->p[x+1][0], det->p[x+1][1], 255, 10);
            }
            TagMatch tag_match;
            tag_match.p0 = cv::Point2d(det->p[0][0], det->p[0][1]);
            tag_match.p1 = cv::Point2d(det->p[1][0], det->p[1][1]);
            tag_match.p2 = cv::Point2d(det->p[2][0], det->p[2][1]);
            tag_match.p3 = cv::Point2d(det->p[3][0], det->p[3][1]);
            Eigen::Map<Eigen::Matrix3d> H_map(det->H->data);
            tag_match.H = H_map.transpose();
            tag_matches.push_back(tag_match);
            hamm_hist[det->hamming]++;
        }

        apriltag_detections_destroy(detections);

        if (!quiet) {
            timeprofile_display(td->tp);
            printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
            printf("Hamming histogram: ");
            for (int i = 0; i < hamm_hist_max; i++)
                printf("%5d", hamm_hist[i]);
            printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
            printf("\n");
        }

        return tag_matches;
    }
    private:
    int quiet;
    apriltag_family_t *tf;
    apriltag_detector_t *td;
    getopt_t *getopt;
};

class CameraListener {
    public:

    void setDetector(AprilTagDetector* detector) {
        mDetector = detector;
    }

    bool setup() {
        mBotWrapper.reset(new drc::BotWrapper());
        mLcmWrapper.reset(new drc::LcmWrapper(mBotWrapper->getLcm()));
        mLcmWrapper->get()->subscribe("CAMERA", &CameraListener::onCamera, this);

        if (mBotWrapper->getBotParam() == nullptr) {
            printf("Couldn't get bot params\n");
            return false;
        }

        mCamTransLeft = bot_param_get_new_camtrans(mBotWrapper->getBotParam(),"CAMERA_LEFT");

        K = Eigen::Matrix3d::Identity();

        K(0,0) = bot_camtrans_get_focal_length_x(mCamTransLeft);
        K(1,1) = bot_camtrans_get_focal_length_y(mCamTransLeft);
        K(0,2) = bot_camtrans_get_principal_x(mCamTransLeft);
        K(1,2) = bot_camtrans_get_principal_y(mCamTransLeft);

        std::cout << K << std::endl;
        Kinv = K.inverse();
        std::cout << Kinv << std::endl;

        return true;
    }

    void start() {
        mLcmWrapper->startHandleThread(true);
    }

    void onCamera(const lcm::ReceiveBuffer* buffer, const std::string& channel,
                const bot_core::images_t* msg) {
        cv::Mat image;
        decodeImage(msg, image);
        image_u8_t *image_u8 = fromCvMat(image);

        std::vector<TagMatch> tags = mDetector->detectTags(image_u8);
        cv::cvtColor(image, image, CV_GRAY2RGB);
        for (int i = 0; i < tags.size(); i++) {

            cv::line(image, tags[i].p0, tags[i].p1, cv::Scalar(255,0,0), 2, CV_AA);
            cv::line(image, tags[i].p1, tags[i].p2, cv::Scalar(0,255,0), 2, CV_AA);
            cv::line(image, tags[i].p2, tags[i].p3, cv::Scalar(0,0,255), 2, CV_AA);
            cv::line(image, tags[i].p3, tags[i].p0, cv::Scalar(0,0,255), 2, CV_AA);

            Eigen::Vector3d x_axis(2,0,1);
            Eigen::Vector3d y_axis(0,2,1);
            Eigen::Vector3d origin(0,0,1);

            Eigen::Vector3d px = tags[i].H * x_axis;
            Eigen::Vector3d py = tags[i].H * y_axis;
            Eigen::Vector3d o  = tags[i].H * origin;

            px/= px[2];
            py/= py[2];
            o/= o[2];

            cv::line(image, cv::Point2d(o[0], o[1]), cv::Point2d(px[0], px[1]), cv::Scalar(255,0,255), 1, CV_AA);
            cv::line(image, cv::Point2d(o[0], o[1]), cv::Point2d(py[0], py[1]), cv::Scalar(255,255,0), 1, CV_AA);
        }
        cv::imshow("detections", image);
        cv::waitKey(1);
        image_u8_destroy(image_u8);
    }

    void decodeImage(const bot_core::images_t* msg, cv::Mat & decoded_image) {
        bot_core::image_t* leftImage = NULL;
        for (int i = 0; i < msg->n_images; ++i) {
          if (msg->image_types[i] == bot_core::images_t::LEFT) {
            leftImage = (bot_core::image_t*)(&msg->images[i]);
            decoded_image = leftImage->pixelformat == leftImage->PIXEL_FORMAT_MJPEG ?
                cv::imdecode(cv::Mat(leftImage->data), -1) :
                cv::Mat(leftImage->height, leftImage->width, CV_8UC1, leftImage->data.data());
                if (decoded_image.channels() > 1) {
                    cv::cvtColor(decoded_image, decoded_image, CV_RGB2GRAY);
                }
                break;
          }
        }
    }

    image_u8_t *fromCvMat(const cv::Mat & img) {
        image_u8_t *image_u8 = image_u8_create_alignment(img.cols, img.rows, img.step);
        int size = img.total() * img.elemSize();
        memcpy(image_u8->buf, img.data, size * sizeof(uint8_t));
        return image_u8;
    }

    private:
    AprilTagDetector *mDetector;
    drc::LcmWrapper::Ptr mLcmWrapper;
    drc::BotWrapper::Ptr mBotWrapper;
    BotCamTrans* mCamTransLeft;
    Eigen::Matrix3d K;
    Eigen::Matrix3d Kinv;

};

int main(int argc, char *argv[])
{
    getopt_t *getopt = getopt_create();

    getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
    getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
    getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
    getopt_add_int(getopt, '\0', "border", "1", "Set tag family border size");
    getopt_add_int(getopt, 't', "threads", "4", "Use this many CPU threads");
    getopt_add_double(getopt, 'x', "decimate", "1.0", "Decimate input image by this factor");
    getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");
    getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
    getopt_add_bool(getopt, '1', "refine-decode", 0, "Spend more time trying to decode tags");
    getopt_add_bool(getopt, '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");

    if (!getopt_parse(getopt, argc, argv, 1) || getopt_get_bool(getopt, "help")) {
        printf("Usage: %s [options]\n", argv[0]);
        getopt_do_usage(getopt);
        exit(0);
    }

    AprilTagDetector tag_detector(getopt);
    CameraListener camera_listener;

    if (camera_listener.setup()) {
        camera_listener.setDetector(&tag_detector);
        camera_listener.start();
    }

    return 0;
}
<commit_msg>used perspective resection to get the homogeneous transform from the camera to the tag<commit_after>#include <stdio.h>
#include <stdint.h>
#include <inttypes.h>
#include <ctype.h>
#include <unistd.h>

#include <opencv2/opencv.hpp>

#include <AprilTags/apriltag.h>
#include <AprilTags/common/image_u8.h>
#include <AprilTags/tag36h11.h>
#include <AprilTags/common/zarray.h>
#include <AprilTags/common/getopt.h>

#include <drc_utils/LcmWrapper.hpp>
#include <drc_utils/BotWrapper.hpp>
#include <lcm/lcm-cpp.hpp>

#include <lcmtypes/drc/robot_state_t.hpp>
#include <lcmtypes/bot_core/images_t.hpp>
#include <lcmtypes/bot_core/image_t.hpp>

#include <bot_core/camtrans.h>
#include <bot_param/param_util.h>

#include <vector>
#include <iostream>

#include <Eigen/Dense>

struct TagMatch {
    cv::Point2d p0, p1, p2, p3;
    Eigen::Matrix3d H;
};

Eigen::Matrix4d getRelativeTransform(TagMatch const& match, Eigen::Matrix3d const & K, double tag_size) {
  std::vector<cv::Point3f> objPts;
  std::vector<cv::Point2f> imgPts;
  double s = tag_size/2.;
  objPts.push_back(cv::Point3f(-s,-s, 0));
  objPts.push_back(cv::Point3f( s,-s, 0));
  objPts.push_back(cv::Point3f( s, s, 0));
  objPts.push_back(cv::Point3f(-s, s, 0));

  imgPts.push_back(match.p0);
  imgPts.push_back(match.p1);
  imgPts.push_back(match.p2);
  imgPts.push_back(match.p3);

  cv::Mat rvec, tvec;
  cv::Matx33f cameraMatrix(
                           K(0,0), 0, K(0,2),
                           0, K(1,1), K(1,2),
                           0,  0,  1);

  cv::Vec4f distParam(0,0,0,0);
  cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
  cv::Matx33d r;
  cv::Rodrigues(rvec, r);
  Eigen::Matrix3d wRo;
  wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2);

  Eigen::Matrix4d T;
  T.topLeftCorner(3,3) = wRo;
  T.col(3).head(3) << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
  T.row(3) << 0,0,0,1;

  return T;
}

class AprilTagDetector {
    public:
    AprilTagDetector(getopt_t *options) : getopt(options) {
        tf = tag36h11_create();
        tf->black_border = getopt_get_int(options, "border");
        td = apriltag_detector_create();
        apriltag_detector_add_family(td, tf);

        td->quad_decimate = getopt_get_double(getopt, "decimate");
        td->quad_sigma = getopt_get_double(getopt, "blur");
        td->nthreads = getopt_get_int(getopt, "threads");
        td->debug = getopt_get_bool(getopt, "debug");
        td->refine_edges = getopt_get_bool(getopt, "refine-edges");
        td->refine_decode = getopt_get_bool(getopt, "refine-decode");
        td->refine_pose = getopt_get_bool(getopt, "refine-pose");

        quiet = getopt_get_bool(getopt, "quiet");
    }

    ~AprilTagDetector() {

        apriltag_detector_destroy(td);
        tag36h11_destroy(tf);
    }

    std::vector<TagMatch> detectTags(image_u8_t *im) {

        const int hamm_hist_max = 10;

        int hamm_hist[hamm_hist_max];
        memset(hamm_hist, 0, sizeof(hamm_hist));
        //image_u8_t *im = image_u8_create_from_pnm(path);

        zarray_t *detections = apriltag_detector_detect(td, im);

        std::vector<TagMatch> tag_matches;
        for (int i = 0; i < zarray_size(detections); i++) {
            apriltag_detection_t *det;
            zarray_get(detections, i, &det);

            if (!quiet)
                printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n",
                       i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);

            for (int x = 0; x < 3 ; x++) {
                image_u8_draw_line(im, det->p[x][0], det->p[x][1], det->p[x+1][0], det->p[x+1][1], 255, 10);
            }
            TagMatch tag_match;
            tag_match.p0 = cv::Point2d(det->p[0][0], det->p[0][1]);
            tag_match.p1 = cv::Point2d(det->p[1][0], det->p[1][1]);
            tag_match.p2 = cv::Point2d(det->p[2][0], det->p[2][1]);
            tag_match.p3 = cv::Point2d(det->p[3][0], det->p[3][1]);
            Eigen::Map<Eigen::Matrix3d> H_map(det->H->data);
            tag_match.H = H_map.transpose();
            tag_matches.push_back(tag_match);
            hamm_hist[det->hamming]++;
        }

        apriltag_detections_destroy(detections);

        if (!quiet) {
            timeprofile_display(td->tp);
            printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
            printf("Hamming histogram: ");
            for (int i = 0; i < hamm_hist_max; i++)
                printf("%5d", hamm_hist[i]);
            printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
            printf("\n");
        }

        return tag_matches;
    }
    private:
    int quiet;
    apriltag_family_t *tf;
    apriltag_detector_t *td;
    getopt_t *getopt;
};

class CameraListener {
    public:

    void setDetector(AprilTagDetector* detector) {
        mDetector = detector;
    }

    bool setup() {
        mBotWrapper.reset(new drc::BotWrapper());
        mLcmWrapper.reset(new drc::LcmWrapper(mBotWrapper->getLcm()));
        mLcmWrapper->get()->subscribe("CAMERA", &CameraListener::onCamera, this);

        if (mBotWrapper->getBotParam() == nullptr) {
            printf("Couldn't get bot params\n");
            return false;
        }

        mCamTransLeft = bot_param_get_new_camtrans(mBotWrapper->getBotParam(),"CAMERA_LEFT");

        K = Eigen::Matrix3d::Identity();

        K(0,0) = bot_camtrans_get_focal_length_x(mCamTransLeft);
        K(1,1) = bot_camtrans_get_focal_length_y(mCamTransLeft);
        K(0,2) = bot_camtrans_get_principal_x(mCamTransLeft);
        K(1,2) = bot_camtrans_get_principal_y(mCamTransLeft);

        std::cout << K << std::endl;
        Kinv = K.inverse();
        std::cout << Kinv << std::endl;

        return true;
    }

    void start() {
        mLcmWrapper->startHandleThread(true);
    }

    void onCamera(const lcm::ReceiveBuffer* buffer, const std::string& channel,
                const bot_core::images_t* msg) {
        cv::Mat image;
        decodeImage(msg, image);
        image_u8_t *image_u8 = fromCvMat(image);

        std::vector<TagMatch> tags = mDetector->detectTags(image_u8);
        cv::cvtColor(image, image, CV_GRAY2RGB);
        for (int i = 0; i < tags.size(); i++) {

            cv::line(image, tags[i].p0, tags[i].p1, cv::Scalar(255,0,0), 2, CV_AA);
            cv::line(image, tags[i].p1, tags[i].p2, cv::Scalar(0,255,0), 2, CV_AA);
            cv::line(image, tags[i].p2, tags[i].p3, cv::Scalar(0,0,255), 2, CV_AA);
            cv::line(image, tags[i].p3, tags[i].p0, cv::Scalar(0,0,255), 2, CV_AA);

            Eigen::Vector3d x_axis(2,0,1);
            Eigen::Vector3d y_axis(0,2,1);
            Eigen::Vector3d origin(0,0,1);

            Eigen::Vector3d px = tags[i].H * x_axis;
            Eigen::Vector3d py = tags[i].H * y_axis;
            Eigen::Vector3d o  = tags[i].H * origin;

            px/= px[2];
            py/= py[2];
            o/= o[2];

            cv::line(image, cv::Point2d(o[0], o[1]), cv::Point2d(px[0], px[1]), cv::Scalar(255,0,255), 1, CV_AA);
            cv::line(image, cv::Point2d(o[0], o[1]), cv::Point2d(py[0], py[1]), cv::Scalar(255,255,0), 1, CV_AA);

            auto T = getRelativeTransform(tags[i], K, 0.165);

            //TODO: publish this transform on bot-frames
        }
        cv::imshow("detections", image);
        cv::waitKey(1);
        image_u8_destroy(image_u8);
    }

    void decodeImage(const bot_core::images_t* msg, cv::Mat & decoded_image) {
        bot_core::image_t* leftImage = NULL;
        for (int i = 0; i < msg->n_images; ++i) {
          if (msg->image_types[i] == bot_core::images_t::LEFT) {
            leftImage = (bot_core::image_t*)(&msg->images[i]);
            decoded_image = leftImage->pixelformat == leftImage->PIXEL_FORMAT_MJPEG ?
                cv::imdecode(cv::Mat(leftImage->data), -1) :
                cv::Mat(leftImage->height, leftImage->width, CV_8UC1, leftImage->data.data());
                if (decoded_image.channels() > 1) {
                    cv::cvtColor(decoded_image, decoded_image, CV_RGB2GRAY);
                }
                break;
          }
        }
    }

    image_u8_t *fromCvMat(const cv::Mat & img) {
        image_u8_t *image_u8 = image_u8_create_alignment(img.cols, img.rows, img.step);
        int size = img.total() * img.elemSize();
        memcpy(image_u8->buf, img.data, size * sizeof(uint8_t));
        return image_u8;
    }

    private:
    AprilTagDetector *mDetector;
    drc::LcmWrapper::Ptr mLcmWrapper;
    drc::BotWrapper::Ptr mBotWrapper;
    BotCamTrans* mCamTransLeft;
    Eigen::Matrix3d K;
    Eigen::Matrix3d Kinv;

};

int main(int argc, char *argv[])
{
    getopt_t *getopt = getopt_create();

    getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
    getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
    getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
    getopt_add_int(getopt, '\0', "border", "1", "Set tag family border size");
    getopt_add_int(getopt, 't', "threads", "4", "Use this many CPU threads");
    getopt_add_double(getopt, 'x', "decimate", "1.0", "Decimate input image by this factor");
    getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");
    getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
    getopt_add_bool(getopt, '1', "refine-decode", 0, "Spend more time trying to decode tags");
    getopt_add_bool(getopt, '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");

    if (!getopt_parse(getopt, argc, argv, 1) || getopt_get_bool(getopt, "help")) {
        printf("Usage: %s [options]\n", argv[0]);
        getopt_do_usage(getopt);
        exit(0);
    }

    AprilTagDetector tag_detector(getopt);
    CameraListener camera_listener;

    if (camera_listener.setup()) {
        camera_listener.setDetector(&tag_detector);
        camera_listener.start();
    }

    return 0;
}
<|endoftext|>


<commit_before>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "dense_xw_product_function.h"
#include "dense_tensor.h"
#include "dense_tensor_view.h"
#include <vespa/eval/eval/value.h>
#include <vespa/eval/tensor/tensor.h>
#include <vespa/vespalib/util/exceptions.h>
#include <assert.h>

namespace vespalib::tensor {

namespace {

DenseTensorView::CellsRef
getCellsRef(const eval::Value &value)
{
    const DenseTensorView &denseTensor = static_cast<const DenseTensorView &>(value);
    return denseTensor.cellsRef();
}

void multiDotProduct(const DenseXWProductFunction::Self &self,
                     const XWInput &vectorCells, const XWInput &matrixCells, XWOutput &result)
{
    double *out = result.begin();
    const double *matrixP = matrixCells.cbegin();
    const double * const vectorP = vectorCells.cbegin();
    for (size_t row = 0; row < self._resultSize; ++row) {
        double cell = self._hwAccelerator->dotProduct(vectorP, matrixP, self._vectorSize);
        *out++ = cell;
        matrixP += self._vectorSize;
    }
    assert(out == result.end());
    assert(matrixP == matrixCells.cend());
}

void transposedProduct(const DenseXWProductFunction::Self &self,
                       const XWInput &vectorCells, const XWInput &matrixCells, XWOutput &result)
{
    double *out = result.begin();
    const double * const matrixP = matrixCells.cbegin();
    const double * const vectorP = vectorCells.cbegin();
    for (size_t row = 0; row < self._resultSize; ++row) {
        double cell = 0;
        for (size_t col = 0; col < self._vectorSize; ++col) {
            cell += matrixP[col*self._resultSize + row] * vectorP[col];
        }
        *out++ = cell;
    }
    assert(out == result.end());
}

template <bool commonDimensionInnermost>
void my_op(eval::InterpretedFunction::State &state, uint64_t param) {
    DenseXWProductFunction::Self *self = (DenseXWProductFunction::Self *)(param);

    DenseTensorView::CellsRef vectorCells = getCellsRef(state.peek(1));
    DenseTensorView::CellsRef matrixCells = getCellsRef(state.peek(0));

    if (commonDimensionInnermost) {
        multiDotProduct(*self, vectorCells, matrixCells, self->_outputCells);
    } else {
        transposedProduct(*self, vectorCells, matrixCells, self->_outputCells);
    }
    state.pop_pop_push(self->_outputView);
}

} // namespace vespalib::tensor::<unnamed>

DenseXWProductFunction::Self::Self(const eval::ValueType &resultType,
                                   size_t vectorSize,
                                   size_t resultSize,
                                   Stash &stash)
    : _outputCells(stash.create_array<double>(resultSize)),
      _outputView(resultType, _outputCells),
      _vectorSize(vectorSize),
      _resultSize(resultSize),
      _hwAccelerator(hwaccelrated::IAccelrated::getAccelrator())
{}

DenseXWProductFunction::DenseXWProductFunction(const eval::ValueType &resultType,
                                               const eval::TensorFunction &vector_in,
                                               const eval::TensorFunction &matrix_in,
                                               size_t vectorSize,
                                               size_t resultSize,
                                               bool matrixHasCommonDimensionInnermost)
    : eval::tensor_function::Op2(resultType, vector_in, matrix_in),
      _vectorSize(vectorSize),
      _resultSize(resultSize),
      _commonDimensionInnermost(matrixHasCommonDimensionInnermost)
{}

namespace {

} // namespace <unnamed>

eval::InterpretedFunction::Instruction
DenseXWProductFunction::compile_self(Stash &stash) const
{
    Self &self = stash.create<Self>(result_type(), _vectorSize, _resultSize, stash);
    if (_commonDimensionInnermost) {
        return eval::InterpretedFunction::Instruction(my_op<true>, (uint64_t)(&self));
    } else {
        return eval::InterpretedFunction::Instruction(my_op<false>, (uint64_t)(&self));
    }
}

}

<commit_msg>minor cleanup<commit_after>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "dense_xw_product_function.h"
#include "dense_tensor.h"
#include "dense_tensor_view.h"
#include <vespa/eval/eval/value.h>
#include <vespa/eval/tensor/tensor.h>
#include <vespa/vespalib/util/exceptions.h>
#include <assert.h>

namespace vespalib::tensor {

namespace {

DenseTensorView::CellsRef
getCellsRef(const eval::Value &value)
{
    const DenseTensorView &denseTensor = static_cast<const DenseTensorView &>(value);
    return denseTensor.cellsRef();
}

void multiDotProduct(const DenseXWProductFunction::Self &self,
                     const XWInput &vectorCells, const XWInput &matrixCells, XWOutput &result)
{
    double *out = result.begin();
    const double *matrixP = matrixCells.cbegin();
    const double * const vectorP = vectorCells.cbegin();
    for (size_t row = 0; row < self._resultSize; ++row) {
        double cell = self._hwAccelerator->dotProduct(vectorP, matrixP, self._vectorSize);
        *out++ = cell;
        matrixP += self._vectorSize;
    }
    assert(out == result.end());
    assert(matrixP == matrixCells.cend());
}

void transposedProduct(const DenseXWProductFunction::Self &self,
                       const XWInput &vectorCells, const XWInput &matrixCells, XWOutput &result)
{
    double *out = result.begin();
    const double * const matrixP = matrixCells.cbegin();
    const double * const vectorP = vectorCells.cbegin();
    for (size_t row = 0; row < self._resultSize; ++row) {
        double cell = 0;
        for (size_t col = 0; col < self._vectorSize; ++col) {
            cell += matrixP[col*self._resultSize + row] * vectorP[col];
        }
        *out++ = cell;
    }
    assert(out == result.end());
}

template <bool commonDimensionInnermost>
void my_op(eval::InterpretedFunction::State &state, uint64_t param) {
    DenseXWProductFunction::Self *self = (DenseXWProductFunction::Self *)(param);

    DenseTensorView::CellsRef vectorCells = getCellsRef(state.peek(1));
    DenseTensorView::CellsRef matrixCells = getCellsRef(state.peek(0));

    if (commonDimensionInnermost) {
        multiDotProduct(*self, vectorCells, matrixCells, self->_outputCells);
    } else {
        transposedProduct(*self, vectorCells, matrixCells, self->_outputCells);
    }
    state.pop_pop_push(self->_outputView);
}

} // namespace vespalib::tensor::<unnamed>

DenseXWProductFunction::Self::Self(const eval::ValueType &resultType,
                                   size_t vectorSize,
                                   size_t resultSize,
                                   Stash &stash)
    : _outputCells(stash.create_array<double>(resultSize)),
      _outputView(resultType, _outputCells),
      _vectorSize(vectorSize),
      _resultSize(resultSize),
      _hwAccelerator(hwaccelrated::IAccelrated::getAccelrator())
{}

DenseXWProductFunction::DenseXWProductFunction(const eval::ValueType &resultType,
                                               const eval::TensorFunction &vector_in,
                                               const eval::TensorFunction &matrix_in,
                                               size_t vectorSize,
                                               size_t resultSize,
                                               bool matrixHasCommonDimensionInnermost)
    : eval::tensor_function::Op2(resultType, vector_in, matrix_in),
      _vectorSize(vectorSize),
      _resultSize(resultSize),
      _commonDimensionInnermost(matrixHasCommonDimensionInnermost)
{}

eval::InterpretedFunction::Instruction
DenseXWProductFunction::compile_self(Stash &stash) const
{
    Self &self = stash.create<Self>(result_type(), _vectorSize, _resultSize, stash);
    auto op = _commonDimensionInnermost ? my_op<true> : my_op<false>;
    return eval::InterpretedFunction::Instruction(op, (uint64_t)(&self));
}

} // namespace vespalib::tensor
<|endoftext|>


<commit_before><commit_msg>added tag config parsing<commit_after><|endoftext|>


<commit_before>#pragma once

#include "types.hpp"
#include <vector>

namespace manipulator {
class modifier_flag_manager final {
public:
  modifier_flag_manager(void) {
    states_.resize(static_cast<size_t>(krbn::modifier_flag::prepared_modifier_flag_end_));

    states_[static_cast<size_t>(krbn::modifier_flag::caps_lock)] = std::make_unique<state>("caps lock", "⇪");
    states_[static_cast<size_t>(krbn::modifier_flag::left_control)] = std::make_unique<state>("control", "⌃");
    states_[static_cast<size_t>(krbn::modifier_flag::left_shift)] = std::make_unique<state>("shift", "⇧");
    states_[static_cast<size_t>(krbn::modifier_flag::left_option)] = std::make_unique<state>("option", "⌥");
    states_[static_cast<size_t>(krbn::modifier_flag::left_command)] = std::make_unique<state>("command", "⌘");
    states_[static_cast<size_t>(krbn::modifier_flag::right_control)] = std::make_unique<state>("control", "⌃");
    states_[static_cast<size_t>(krbn::modifier_flag::right_shift)] = std::make_unique<state>("shift", "⇧");
    states_[static_cast<size_t>(krbn::modifier_flag::right_option)] = std::make_unique<state>("option", "⌥");
    states_[static_cast<size_t>(krbn::modifier_flag::right_command)] = std::make_unique<state>("command", "⌘");
    states_[static_cast<size_t>(krbn::modifier_flag::fn)] = std::make_unique<state>("fn", "fn");
  }

  void reset(void) {
    for (const auto& s : states_) {
      if (s) {
        s->reset();
      }
    }
  }

  void unlock(void) {
    for (const auto& s : states_) {
      if (s) {
        s->unlock();
      }
    }
  }

  enum class operation {
    increase,
    decrease,
    lock,
    unlock,
    toggle_lock,
  };

  void manipulate(krbn::modifier_flag k, operation operation) {
    auto i = static_cast<size_t>(k);
    if (states_[i]) {
      switch (operation) {
      case operation::increase:
        states_[i]->increase();
        break;
      case operation::decrease:
        states_[i]->decrease();
        break;
      case operation::lock:
        states_[i]->lock();
        break;
      case operation::unlock:
        states_[i]->unlock();
        break;
      case operation::toggle_lock:
        states_[i]->toggle_lock();
        break;
      }
    }
  }

  bool pressed(krbn::modifier_flag m) const {
    if (m == krbn::modifier_flag::zero) {
      return true;
    }

    auto i = static_cast<size_t>(m);
    if (i < states_.size() && states_[i]) {
      return states_[i]->pressed();
    }
    return false;
  }

  bool pressed(const std::vector<krbn::modifier_flag>& modifier_flags) {
    // return true if all modifier flags are pressed.
    for (const auto& m : modifier_flags) {
      if (!pressed(m)) {
        return false;
      }
    }
    return true;
  }

  uint8_t get_hid_report_bits(void) const {
    uint8_t bits = 0;

    if (pressed(krbn::modifier_flag::left_control)) {
      bits |= (0x1 << 0);
    }
    if (pressed(krbn::modifier_flag::left_shift)) {
      bits |= (0x1 << 1);
    }
    if (pressed(krbn::modifier_flag::left_option)) {
      bits |= (0x1 << 2);
    }
    if (pressed(krbn::modifier_flag::left_command)) {
      bits |= (0x1 << 3);
    }
    if (pressed(krbn::modifier_flag::right_control)) {
      bits |= (0x1 << 4);
    }
    if (pressed(krbn::modifier_flag::right_shift)) {
      bits |= (0x1 << 5);
    }
    if (pressed(krbn::modifier_flag::right_option)) {
      bits |= (0x1 << 6);
    }
    if (pressed(krbn::modifier_flag::right_command)) {
      bits |= (0x1 << 7);
    }

    return bits;
  }

  IOOptionBits get_io_option_bits(void) const {
    IOOptionBits bits = 0;
    if (pressed(krbn::modifier_flag::caps_lock)) {
      bits |= NX_ALPHASHIFTMASK;
    }
    if (pressed(krbn::modifier_flag::left_control) ||
        pressed(krbn::modifier_flag::right_control)) {
      bits |= NX_CONTROLMASK;
    }
    if (pressed(krbn::modifier_flag::left_shift) ||
        pressed(krbn::modifier_flag::right_shift)) {
      bits |= NX_SHIFTMASK;
    }
    if (pressed(krbn::modifier_flag::left_option) ||
        pressed(krbn::modifier_flag::right_option)) {
      bits |= NX_ALTERNATEMASK;
    }
    if (pressed(krbn::modifier_flag::left_command) ||
        pressed(krbn::modifier_flag::right_command)) {
      bits |= NX_COMMANDMASK;
    }
    if (pressed(krbn::modifier_flag::fn)) {
      bits |= NX_SECONDARYFNMASK;
    }
    return bits;
  }

private:
  class state final {
  public:
    state(const std::string& name, const std::string& symbol) : name_(name),
                                                                symbol_(symbol),
                                                                count_(0),
                                                                lock_count_(0) {}

    const std::string& get_name(void) const { return name_; }
    const std::string& get_symbol(void) const { return symbol_; }

    bool pressed(void) const { return (count_ + lock_count_) > 0; }

    void reset(void) {
      count_ = 0;
    }

    void increase(void) {
      ++count_;
    }

    void decrease(void) {
      --count_;
    }

    void lock(void) {
      lock_count_ = 1;
    }

    void unlock(void) {
      lock_count_ = 0;
    }

    void toggle_lock(void) {
      lock_count_ = lock_count_ == 0 ? 1 : 0;
    }

  private:
    std::string name_;
    std::string symbol_;
    int count_;
    int lock_count_;
  };

  std::vector<std::unique_ptr<state>> states_;
};
}
<commit_msg>add get_cg_event_flags<commit_after>#pragma once

#include "types.hpp"
#include <CoreGraphics/CoreGraphics.h>
#include <vector>

namespace manipulator {
class modifier_flag_manager final {
public:
  modifier_flag_manager(void) {
    states_.resize(static_cast<size_t>(krbn::modifier_flag::prepared_modifier_flag_end_));

    states_[static_cast<size_t>(krbn::modifier_flag::caps_lock)] = std::make_unique<state>("caps lock", "⇪");
    states_[static_cast<size_t>(krbn::modifier_flag::left_control)] = std::make_unique<state>("control", "⌃");
    states_[static_cast<size_t>(krbn::modifier_flag::left_shift)] = std::make_unique<state>("shift", "⇧");
    states_[static_cast<size_t>(krbn::modifier_flag::left_option)] = std::make_unique<state>("option", "⌥");
    states_[static_cast<size_t>(krbn::modifier_flag::left_command)] = std::make_unique<state>("command", "⌘");
    states_[static_cast<size_t>(krbn::modifier_flag::right_control)] = std::make_unique<state>("control", "⌃");
    states_[static_cast<size_t>(krbn::modifier_flag::right_shift)] = std::make_unique<state>("shift", "⇧");
    states_[static_cast<size_t>(krbn::modifier_flag::right_option)] = std::make_unique<state>("option", "⌥");
    states_[static_cast<size_t>(krbn::modifier_flag::right_command)] = std::make_unique<state>("command", "⌘");
    states_[static_cast<size_t>(krbn::modifier_flag::fn)] = std::make_unique<state>("fn", "fn");
  }

  void reset(void) {
    for (const auto& s : states_) {
      if (s) {
        s->reset();
      }
    }
  }

  void unlock(void) {
    for (const auto& s : states_) {
      if (s) {
        s->unlock();
      }
    }
  }

  enum class operation {
    increase,
    decrease,
    lock,
    unlock,
    toggle_lock,
  };

  void manipulate(krbn::modifier_flag k, operation operation) {
    auto i = static_cast<size_t>(k);
    if (states_[i]) {
      switch (operation) {
      case operation::increase:
        states_[i]->increase();
        break;
      case operation::decrease:
        states_[i]->decrease();
        break;
      case operation::lock:
        states_[i]->lock();
        break;
      case operation::unlock:
        states_[i]->unlock();
        break;
      case operation::toggle_lock:
        states_[i]->toggle_lock();
        break;
      }
    }
  }

  bool pressed(krbn::modifier_flag m) const {
    if (m == krbn::modifier_flag::zero) {
      return true;
    }

    auto i = static_cast<size_t>(m);
    if (i < states_.size() && states_[i]) {
      return states_[i]->pressed();
    }
    return false;
  }

  bool pressed(const std::vector<krbn::modifier_flag>& modifier_flags) {
    // return true if all modifier flags are pressed.
    for (const auto& m : modifier_flags) {
      if (!pressed(m)) {
        return false;
      }
    }
    return true;
  }

  uint8_t get_hid_report_bits(void) const {
    uint8_t bits = 0;

    if (pressed(krbn::modifier_flag::left_control)) {
      bits |= (0x1 << 0);
    }
    if (pressed(krbn::modifier_flag::left_shift)) {
      bits |= (0x1 << 1);
    }
    if (pressed(krbn::modifier_flag::left_option)) {
      bits |= (0x1 << 2);
    }
    if (pressed(krbn::modifier_flag::left_command)) {
      bits |= (0x1 << 3);
    }
    if (pressed(krbn::modifier_flag::right_control)) {
      bits |= (0x1 << 4);
    }
    if (pressed(krbn::modifier_flag::right_shift)) {
      bits |= (0x1 << 5);
    }
    if (pressed(krbn::modifier_flag::right_option)) {
      bits |= (0x1 << 6);
    }
    if (pressed(krbn::modifier_flag::right_command)) {
      bits |= (0x1 << 7);
    }

    return bits;
  }

  IOOptionBits get_io_option_bits(void) const {
    IOOptionBits bits = 0;
    if (pressed(krbn::modifier_flag::caps_lock)) {
      bits |= NX_ALPHASHIFTMASK;
    }
    if (pressed(krbn::modifier_flag::left_control) ||
        pressed(krbn::modifier_flag::right_control)) {
      bits |= NX_CONTROLMASK;
    }
    if (pressed(krbn::modifier_flag::left_shift) ||
        pressed(krbn::modifier_flag::right_shift)) {
      bits |= NX_SHIFTMASK;
    }
    if (pressed(krbn::modifier_flag::left_option) ||
        pressed(krbn::modifier_flag::right_option)) {
      bits |= NX_ALTERNATEMASK;
    }
    if (pressed(krbn::modifier_flag::left_command) ||
        pressed(krbn::modifier_flag::right_command)) {
      bits |= NX_COMMANDMASK;
    }
    if (pressed(krbn::modifier_flag::fn)) {
      bits |= NX_SECONDARYFNMASK;
    }
    return bits;
  }

  CGEventFlags get_cg_event_flags(CGEventFlags original_flags) const {
    original_flags = static_cast<CGEventFlags>(original_flags & ~(kCGEventFlagMaskAlphaShift |
                                                                  kCGEventFlagMaskControl |
                                                                  kCGEventFlagMaskShift |
                                                                  kCGEventFlagMaskAlternate |
                                                                  kCGEventFlagMaskCommand |
                                                                  kCGEventFlagMaskSecondaryFn));

    if (pressed(krbn::modifier_flag::caps_lock)) {
      original_flags = static_cast<CGEventFlags>(original_flags | kCGEventFlagMaskAlphaShift);
    }
    if (pressed(krbn::modifier_flag::left_control) ||
        pressed(krbn::modifier_flag::right_control)) {
      original_flags = static_cast<CGEventFlags>(original_flags | kCGEventFlagMaskControl);
    }
    if (pressed(krbn::modifier_flag::left_shift) ||
        pressed(krbn::modifier_flag::right_shift)) {
      original_flags = static_cast<CGEventFlags>(original_flags | kCGEventFlagMaskShift);
    }
    if (pressed(krbn::modifier_flag::left_option) ||
        pressed(krbn::modifier_flag::right_option)) {
      original_flags = static_cast<CGEventFlags>(original_flags | kCGEventFlagMaskAlternate);
    }
    if (pressed(krbn::modifier_flag::left_command) ||
        pressed(krbn::modifier_flag::right_command)) {
      original_flags = static_cast<CGEventFlags>(original_flags | kCGEventFlagMaskCommand);
    }
    if (pressed(krbn::modifier_flag::fn)) {
      original_flags = static_cast<CGEventFlags>(original_flags | kCGEventFlagMaskSecondaryFn);
    }
    return original_flags;
  }

private:
  class state final {
  public:
    state(const std::string& name, const std::string& symbol) : name_(name),
                                                                symbol_(symbol),
                                                                count_(0),
                                                                lock_count_(0) {}

    const std::string& get_name(void) const { return name_; }
    const std::string& get_symbol(void) const { return symbol_; }

    bool pressed(void) const { return (count_ + lock_count_) > 0; }

    void reset(void) {
      count_ = 0;
    }

    void increase(void) {
      ++count_;
    }

    void decrease(void) {
      --count_;
    }

    void lock(void) {
      lock_count_ = 1;
    }

    void unlock(void) {
      lock_count_ = 0;
    }

    void toggle_lock(void) {
      lock_count_ = lock_count_ == 0 ? 1 : 0;
    }

  private:
    std::string name_;
    std::string symbol_;
    int count_;
    int lock_count_;
  };

  std::vector<std::unique_ptr<state>> states_;
};
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: outleeng.hxx,v $
 *
 *  $Revision: 1.9 $
 *
 *  last change: $Author: kz $ $Date: 2005-10-05 13:25:43 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef _OUTLEENG_HXX
#define _OUTLEENG_HXX

#ifndef _OUTLINER_HXX
#include <outliner.hxx>
#endif
#ifndef _EDITENG_HXX
#include <editeng.hxx>
#endif

typedef EENotify* EENotifyPtr;
SV_DECL_PTRARR_DEL( NotifyList, EENotifyPtr, 1, 1 );

class OutlinerEditEng : public EditEngine
{
    Outliner*           pOwner;

public:
                        OutlinerEditEng( Outliner* pOwner, SfxItemPool* pPool );
                        ~OutlinerEditEng();

    virtual void        PaintingFirstLine( USHORT nPara, const Point& rStartPos, long nBaseLineY, const Point& rOrigin, short nOrientation, OutputDevice* pOutDev );

    virtual void        ParagraphInserted( USHORT nNewParagraph );
    virtual void        ParagraphDeleted( USHORT nDeletedParagraph );

    // #101498#
    virtual void        DrawingText(const Point& rStartPos, const XubString& rText, USHORT nTextStart, USHORT nTextLen, const sal_Int32* pDXArray, const SvxFont& rFont, USHORT nPara, USHORT nIndex, BYTE nRightToLeft);

    virtual void        StyleSheetChanged( SfxStyleSheet* pStyle );
    virtual void        ParaAttribsChanged( USHORT nPara );
    virtual void        ParagraphHeightChanged( USHORT nPara );
    virtual BOOL        SpellNextDocument();
    virtual XubString   GetUndoComment( USHORT nUndoId ) const;

    // for text conversion
    virtual BOOL        ConvertNextDocument();

    virtual void        FieldClicked( const SvxFieldItem& rField, USHORT nPara, USHORT nPos );
    virtual void        FieldSelected( const SvxFieldItem& rField, USHORT nPara, USHORT nPos );
    virtual XubString   CalcFieldValue( const SvxFieldItem& rField, USHORT nPara, USHORT nPos, Color*& rTxtColor, Color*& rFldColor );

    virtual Rectangle   GetBulletArea( USHORT nPara );

    // belongs into class Outliner, move there before incompatible update!
    Link                aOutlinerNotifyHdl;
    NotifyList          aNotifyCache;
};

#endif

<commit_msg>INTEGRATION: CWS sb59 (1.9.452); FILE MERGED 2006/08/03 13:51:53 cl 1.9.452.1: removed compiler warnings<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: outleeng.hxx,v $
 *
 *  $Revision: 1.10 $
 *
 *  last change: $Author: obo $ $Date: 2006-10-12 13:01:38 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef _OUTLEENG_HXX
#define _OUTLEENG_HXX

#ifndef _OUTLINER_HXX
#include <outliner.hxx>
#endif
#ifndef _EDITENG_HXX
#include <editeng.hxx>
#endif

typedef EENotify* EENotifyPtr;
SV_DECL_PTRARR_DEL( NotifyList, EENotifyPtr, 1, 1 )

class OutlinerEditEng : public EditEngine
{
    Outliner*           pOwner;

public:
                        OutlinerEditEng( Outliner* pOwner, SfxItemPool* pPool );
                        ~OutlinerEditEng();

    virtual void        PaintingFirstLine( USHORT nPara, const Point& rStartPos, long nBaseLineY, const Point& rOrigin, short nOrientation, OutputDevice* pOutDev );

    virtual void        ParagraphInserted( USHORT nNewParagraph );
    virtual void        ParagraphDeleted( USHORT nDeletedParagraph );

    // #101498#
    virtual void        DrawingText(const Point& rStartPos, const XubString& rText, USHORT nTextStart, USHORT nTextLen, const sal_Int32* pDXArray, const SvxFont& rFont, USHORT nPara, USHORT nIndex, BYTE nRightToLeft);

    virtual void        StyleSheetChanged( SfxStyleSheet* pStyle );
    virtual void        ParaAttribsChanged( USHORT nPara );
    virtual void        ParagraphHeightChanged( USHORT nPara );
    virtual BOOL        SpellNextDocument();
    virtual XubString   GetUndoComment( USHORT nUndoId ) const;

    // for text conversion
    virtual BOOL        ConvertNextDocument();

    virtual void        FieldClicked( const SvxFieldItem& rField, USHORT nPara, USHORT nPos );
    virtual void        FieldSelected( const SvxFieldItem& rField, USHORT nPara, USHORT nPos );
    virtual XubString   CalcFieldValue( const SvxFieldItem& rField, USHORT nPara, USHORT nPos, Color*& rTxtColor, Color*& rFldColor );

    virtual Rectangle   GetBulletArea( USHORT nPara );

    // belongs into class Outliner, move there before incompatible update!
    Link                aOutlinerNotifyHdl;
    NotifyList          aNotifyCache;
};

#endif

<|endoftext|>


<commit_before>
#include "LogCentral.h" // Own interface

namespace au
{

    LogCentral::LogCentral( std::string _host , int _port , std::string _local_file )
    : token("PermanentSocketConnection")
    , token_plugins("token_plugins")
    , token_current_thread("token_current_thread")
    {
        host = _host;
        port = _port;
        local_file = _local_file;

        // By default both connections are NULL
        socket_connection = NULL;
        local_file_descriptor = NULL;

        // Connect the next time a trace is sent
        time_reconnect = 0;

        // No current thread by defautl
        current_thread_activated = false;
    }

    LogCentral::~LogCentral()
    {
        close_socket_connection();
        close_local_file();
    }

    void LogCentral::close_socket_connection()
    {
        if( socket_connection )
        {
            socket_connection->close();
            delete socket_connection;
            socket_connection = NULL;

            // This is the time counter since last connection
            time_since_last_connection.reset();

            // Force try reconnect in next log
            time_reconnect = 0;
        }
    }

    void LogCentral::close_local_file()
    {
        if( local_file_descriptor )
        {
            local_file_descriptor->close();
            delete local_file_descriptor;
            local_file_descriptor = NULL;
        }
    }

    void LogCentral::set_host_and_port( std::string _host , int _port )
    {
        au::TokenTaker tt(&token);

        // Set the new address
        host = _host;
        port = _port;

        // Close previous connections ( if any )
        close_socket_connection();
        close_local_file();

        // Connect the next time a trace is sent
        time_reconnect = 0;
    }

    void LogCentral::write( Log* log )
    {
        // If we are the writing thread, just return ( otherwise it will be locked )
        // If the system is taken, just wait
        while( true )
        {
            {
                // Mutex protection
                au::TokenTaker tt(&token_current_thread);

                // Get my thread id
                pthread_t my_thread_id = pthread_self();

                // Check if I am blocking this log
                if( current_thread_activated )
                {
                    if( current_thread == my_thread_id )
                        return; // Secondary log...
                }
                else
                {
                    current_thread = my_thread_id;
                    break;
                }
            }

            // Sleep waiting to send traces
            usleep(10000);
        }

        // Real write operation
        write_to_plugins(log);
        write_to_server_or_file(log); // Log is accumulated and finally removed

        {
            // Mutex protection
            au::TokenTaker tt(&token_current_thread);

            // Deactivate this lock
            current_thread_activated = false;
        }

    }

    void LogCentral::addPlugin( LogPlugin* p )
    {
        au::TokenTaker tt(&token_plugins);
        plugins.insert(p);
    }

    void LogCentral::removePlugin( LogPlugin* p )
    {
        au::TokenTaker tt(&token_plugins);
        plugins.erase(p);
    }

    void LogCentral::write_to_plugins( Log *log )
    {
        au::TokenTaker tt(&token_plugins);

        au::set<LogPlugin>::iterator it;
        for( it = plugins.begin() ; it != plugins.end() ; it++ )
            (*it)->process_log(log);
    }

    void LogCentral::write_to_server_or_file( Log *log )
    {
        au::TokenTaker tt(&token);

        // Check if the estabished connection should be canceled
        if( socket_connection && socket_connection->isDisconnected() )
            close_socket_connection();

        // Reconnect to server if necessary
        size_t time = time_since_last_connection.diffTimeInSeconds();
        if( !socket_connection && ( host != "" )  && ( time >= time_reconnect ) )
        {
            // Try new connection with the server
            au::Status s = au::SocketConnection::newSocketConnection( host , port , &socket_connection );

            if( s != au::OK )
            {
                // Set the next time_reconnect
                if( time == 0 )
                    time_reconnect = 120;

                if( time_reconnect < time )
                    time_reconnect = time;

                time_reconnect *= 2; // Increase time to reconnect

                size_t next_try_time = time_reconnect - time;

                LM_W(("Not possible to connect with log server %s:%d (%s). Next try in %s"
                      , host.c_str() , port, au::status(s) , au::str_time(next_try_time).c_str() ));
            }
            else
            {
                if( time  > 10 )
                    LM_LW(("Connected to log server after %s disconnected" , au::str_time(time).c_str() ));
            }
        }

        // Try socket first...
        if( socket_connection )
        {
            if( log->write( socket_connection ) )
                return;

            // It was not possible to sent to server, close and remove socket connection
            close_socket_connection();
        }

        // Make sure, local file is ready to write

        if( !local_file_descriptor )
        {
            int fd = open( local_file.c_str() , O_WRONLY | O_CREAT , 0644 );
            LM_LT(LmtFileDescriptors, ("Open FileDescriptor fd:%d", fd));

            if( fd >= 0 )
                local_file_descriptor = new FileDescriptor("local_log", fd );
            else
            {
                LM_LW(("Not possible to open local log file %s. Logs will be definitely lost", local_file.c_str() ));
                return;
            }
        }

        if( !log->write(local_file_descriptor ) )
        {
            close_local_file();
            return;
        }

    }

}<commit_msg>Small bug when detecting conflicts in logs requests<commit_after>
#include "LogCentral.h" // Own interface

namespace au
{

    LogCentral::LogCentral( std::string _host , int _port , std::string _local_file )
    : token("PermanentSocketConnection")
    , token_plugins("token_plugins")
    , token_current_thread("token_current_thread")
    {
        host = _host;
        port = _port;
        local_file = _local_file;

        // By default both connections are NULL
        socket_connection = NULL;
        local_file_descriptor = NULL;

        // Connect the next time a trace is sent
        time_reconnect = 0;

        // No current thread by defautl
        current_thread_activated = false;
    }

    LogCentral::~LogCentral()
    {
        close_socket_connection();
        close_local_file();
    }

    void LogCentral::close_socket_connection()
    {
        if( socket_connection )
        {
            socket_connection->close();
            delete socket_connection;
            socket_connection = NULL;

            // This is the time counter since last connection
            time_since_last_connection.reset();

            // Force try reconnect in next log
            time_reconnect = 0;
        }
    }

    void LogCentral::close_local_file()
    {
        if( local_file_descriptor )
        {
            local_file_descriptor->close();
            delete local_file_descriptor;
            local_file_descriptor = NULL;
        }
    }

    void LogCentral::set_host_and_port( std::string _host , int _port )
    {
        au::TokenTaker tt(&token);

        // Set the new address
        host = _host;
        port = _port;

        // Close previous connections ( if any )
        close_socket_connection();
        close_local_file();

        // Connect the next time a trace is sent
        time_reconnect = 0;
    }

    void LogCentral::write( Log* log )
    {
        // If we are the writing thread, just return ( otherwise it will be locked )
        // If the system is taken, just wait
        while( true )
        {
            {
                // Mutex protection
                au::TokenTaker tt(&token_current_thread);

                // Get my thread id
                pthread_t my_thread_id = pthread_self();

                // Check if I am blocking this log
                if( current_thread_activated )
                {
                    if( current_thread == my_thread_id )
                        return; // Secondary log...
                }
                else
                {
                    current_thread = my_thread_id;
                    current_thread_activated = true;
                    break;
                }
            }

            // Sleep waiting to send traces
            usleep(10000);
        }

        // Real write operation
        write_to_plugins(log);
        write_to_server_or_file(log); // Log is accumulated and finally removed

        {
            // Mutex protection
            au::TokenTaker tt(&token_current_thread);

            // Deactivate this lock
            current_thread_activated = false;
        }

    }

    void LogCentral::addPlugin( LogPlugin* p )
    {
        au::TokenTaker tt(&token_plugins);
        plugins.insert(p);
    }

    void LogCentral::removePlugin( LogPlugin* p )
    {
        au::TokenTaker tt(&token_plugins);
        plugins.erase(p);
    }

    void LogCentral::write_to_plugins( Log *log )
    {
        au::TokenTaker tt(&token_plugins);

        au::set<LogPlugin>::iterator it;
        for( it = plugins.begin() ; it != plugins.end() ; it++ )
            (*it)->process_log(log);
    }

    void LogCentral::write_to_server_or_file( Log *log )
    {
        au::TokenTaker tt(&token);

        // Check if the estabished connection should be canceled
        if( socket_connection && socket_connection->isDisconnected() )
            close_socket_connection();

        // Reconnect to server if necessary
        size_t time = time_since_last_connection.diffTimeInSeconds();
        if( !socket_connection && ( host != "" )  && ( time >= time_reconnect ) )
        {
            // Try new connection with the server
            au::Status s = au::SocketConnection::newSocketConnection( host , port , &socket_connection );

            if( s != au::OK )
            {
                // Set the next time_reconnect
                if( time == 0 )
                    time_reconnect = 120;

                if( time_reconnect < time )
                    time_reconnect = time;

                time_reconnect *= 2; // Increase time to reconnect

                size_t next_try_time = time_reconnect - time;

                LM_W(("Not possible to connect with log server %s:%d (%s). Next try in %s"
                      , host.c_str() , port, au::status(s) , au::str_time(next_try_time).c_str() ));
            }
            else
            {
                if( time  > 10 )
                    LM_LW(("Connected to log server after %s disconnected" , au::str_time(time).c_str() ));
            }
        }

        // Try socket first...
        if( socket_connection )
        {
            if( log->write( socket_connection ) )
                return;

            // It was not possible to sent to server, close and remove socket connection
            close_socket_connection();
        }

        // Make sure, local file is ready to write

        if( !local_file_descriptor )
        {
            int fd = open( local_file.c_str() , O_WRONLY | O_CREAT , 0644 );
            LM_LT(LmtFileDescriptors, ("Open FileDescriptor fd:%d", fd));

            if( fd >= 0 )
                local_file_descriptor = new FileDescriptor("local_log", fd );
            else
            {
                LM_LW(("Not possible to open local log file %s. Logs will be definitely lost", local_file.c_str() ));
                return;
            }
        }

        if( !log->write(local_file_descriptor ) )
        {
            close_local_file();
            return;
        }

    }

}
<|endoftext|>


<commit_before>/**
 * @file InertialFilter.cpp
 *
 * @author Xu, Yuan
 *
 */

#include "InertialFilter.h"

using namespace naoth;

InertialFilter::InertialFilter(const MotionBlackBoard& bb, Vector2d& offset):
  theBlackBoard(bb),
  sensorData(bb.theInertialSensorData),
  theOffset(offset),
  calibrateNum(0),
  max_offet(Math::fromDegrees(6))
{
}

InertialPercept InertialFilter::filter()
{
  calibrate();

  InertialPercept result;
  result.data = sensorData.data + theOffset;

  return result;
}

void InertialFilter::calibrate()
{
  if ( theBlackBoard.theMotionStatus.currentMotion == motion::stand
      && !intentionallyMoving() )
  {
    // stand and not moving
    if ( theBlackBoard.theSupportPolygon.mode & (SupportPolygon::DOUBLE | SupportPolygon::DOUBLE_LEFT | SupportPolygon::DOUBLE) )
    {
      // both feet on the ground
      Pose3D foot = theBlackBoard.theKinematicChain.theLinks[KinematicChain::LFoot].M;
      const Pose3D& body = theBlackBoard.theKinematicChain.theLinks[KinematicChain::Torso].M;
      Pose3D bodyInFoot = foot.invert() * body;
      Vector2d expectedAngle;
      expectedAngle.x = bodyInFoot.rotation.getXAngle();
      expectedAngle.y = bodyInFoot.rotation.getYAngle();
      Vector2d diff = sensorData.data - expectedAngle;
      if ( abs(diff.x) < max_offet && abs(diff.y) < max_offet )
      {
        // do calibrate
        theNewOffset = (theNewOffset*calibrateNum - diff) / (calibrateNum+1);
        calibrateNum++;
        if ( calibrateNum % 50 == 0 )
        {
          // every 50 datas
          theOffset = theNewOffset;
        }
        return;
      }
    }
  }

  calibrateNum = 0;
}

// check all request joints' speed, return true if all joints are almost not moving
bool InertialFilter::intentionallyMoving()
{
  const double* jointSpeed = theBlackBoard.theMotorJointData.dp;
  const double* stiffness = theBlackBoard.theMotorJointData.stiffness;
  const double min_speed = Math::fromDegrees(1); // degree per second
  const double min_stiffness = 0.05;
  for( int i=0; i<JointData::numOfJoint; i++)
  {
    if ( stiffness[i] > min_stiffness && abs(jointSpeed[i]) > min_speed )
    {
      return true;
    }
  }

  return false;
}
<commit_msg>bugfix<commit_after>/**
 * @file InertialFilter.cpp
 *
 * @author Xu, Yuan
 *
 */

#include "InertialFilter.h"

using namespace naoth;

InertialFilter::InertialFilter(const MotionBlackBoard& bb, Vector2d& offset):
  theBlackBoard(bb),
  sensorData(bb.theInertialSensorData),
  theOffset(offset),
  calibrateNum(0),
  max_offet(Math::fromDegrees(6))
{
}

InertialPercept InertialFilter::filter()
{
  calibrate();

  InertialPercept result;
  result.data = sensorData.data + theOffset;

  return result;
}

void InertialFilter::calibrate()
{
  if ( theBlackBoard.theMotionStatus.currentMotion == motion::stand
      && !intentionallyMoving() )
  {
    // stand and not moving
    if ( theBlackBoard.theSupportPolygon.mode & (SupportPolygon::DOUBLE | SupportPolygon::DOUBLE_LEFT | SupportPolygon::DOUBLE_RIGHT) )
    {
      // both feet on the ground
      Pose3D foot = theBlackBoard.theKinematicChain.theLinks[KinematicChain::LFoot].M;
      const Pose3D& body = theBlackBoard.theKinematicChain.theLinks[KinematicChain::Torso].M;
      Pose3D bodyInFoot = foot.invert() * body;
      Vector2d expectedAngle;
      expectedAngle.x = bodyInFoot.rotation.getXAngle();
      expectedAngle.y = bodyInFoot.rotation.getYAngle();
      Vector2d diff = sensorData.data - expectedAngle;
      if ( abs(diff.x) < max_offet && abs(diff.y) < max_offet )
      {
        // do calibrate
        theNewOffset = (theNewOffset*calibrateNum - diff) / (calibrateNum+1);
        calibrateNum++;
        if ( calibrateNum % 50 == 0 )
        {
          // every 50 datas
          theOffset = theNewOffset;
        }
        return;
      }//end if
    }//end if
  }//end if

  calibrateNum = 0;
}//end calibrate

// check all request joints' speed, return true if all joints are almost not moving
bool InertialFilter::intentionallyMoving()
{
  // check the joints
  const double* jointSpeed = theBlackBoard.theMotorJointData.dp;
  const double* stiffness = theBlackBoard.theMotorJointData.stiffness;
  const double min_speed = Math::fromDegrees(1); // degree per second
  const double min_stiffness = 0.05;
  for( int i=0; i<JointData::numOfJoint; i++)
  {
    if ( stiffness[i] > min_stiffness && abs(jointSpeed[i]) > min_speed )
    {
      return true;
    }
  }//end for

  // check the gyro

  return false;
}//end intentionallyMoving
<|endoftext|>