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

#include "wallItem.h"

#include <QtWidgets/QAction>
#include <QtWidgets/QGraphicsSceneMouseEvent>

#include <qrkernel/settingsManager.h>
#include <qrutils/mathUtils/geometry.h>

using namespace twoDModel::items;
using namespace qReal;
using namespace graphicsUtils;

const int wallWidth = 10;

WallItem::WallItem(const QPointF &begin, const QPointF &end)
	: SolidItem()
	, mDragged(false)
	, mImage(":/icons/2d_wall.png")
	, mOldX1(0)
	, mOldY1(0)
	, mCellNumbX1(0)
	, mCellNumbY1(0)
	, mCellNumbX2(0)
	, mCellNumbY2(0)
{
	setX1(begin.x());
	setY1(begin.y());
	setX2(end.x());
	setY2(end.y());
	setFlags(ItemIsSelectable | ItemIsMovable);
	setPrivateData();
	setAcceptDrops(true);
}

WallItem *WallItem::clone() const
{
	WallItem * const cloned = new WallItem({x1(), y1()}, {x2(), y2()});
	AbstractItem::copyTo(cloned);
	connect(this, &AbstractItem::positionChanged, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::x1Changed, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::y1Changed, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::x2Changed, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::y2Changed, cloned, &WallItem::recalculateBorders);

	cloned->mCellNumbX1 = mCellNumbX1;
	cloned->mCellNumbY1 = mCellNumbY1;
	cloned->mCellNumbX2 = mCellNumbX2;
	cloned->mCellNumbY2 = mCellNumbY2;

	cloned->mOldX1 = mOldX1;
	cloned->mOldY1 = mOldY1;
	cloned->mDragged = mDragged;
	cloned->mOverlappedWithRobot = mOverlappedWithRobot;
	cloned->mPath = mPath;
	return cloned;
}

QAction *WallItem::wallTool()
{
	QAction * const result = new QAction(QIcon(":/icons/2d_wall.png"), tr("Wall (W)"), nullptr);
	result->setShortcut(QKeySequence(Qt::Key_W));
	return result;
}

void WallItem::setPrivateData()
{
	setZValue(1);
	QPen pen(this->pen());
	pen.setWidth(wallWidth);
	pen.setStyle(Qt::NoPen);
	setPen(pen);
	QBrush brush(this->brush());
	brush.setStyle(Qt::SolidPattern);
	brush.setTextureImage(mImage);
	setBrush(brush);
}

QPointF WallItem::begin() const
{
	return QPointF(x1(), y1()) + scenePos();
}

QPointF WallItem::end() const
{
	return QPointF(x2(), y2()) + scenePos();
}

QRectF WallItem::boundingRect() const
{
	return mLineImpl.boundingRect(x1(), y1(), x2(), y2(), pen().width(), drift);
}

QPainterPath WallItem::shape() const
{
	return mLineImpl.shape(drift, x1(), y1(), x2(), y2());
}

void WallItem::drawItem(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
	Q_UNUSED(option);
	Q_UNUSED(widget);
	painter->drawPath(shape());
	recalculateBorders();
}

void WallItem::drawExtractionForItem(QPainter *painter)
{
	if (!isSelected()) {
		return;
	}

	painter->setPen(QPen(Qt::green));
	mLineImpl.drawExtractionForItem(painter, x1(), y1(), x2(), y2(), drift);
	mLineImpl.drawFieldForResizeItem(painter, resizeDrift, x1(), y1(), x2(), y2());
}

void WallItem::mousePressEvent(QGraphicsSceneMouseEvent * event)
{
	AbstractItem::mousePressEvent(event);

	if (!editable()) {
		return;
	}

	mDragged = (flags() & ItemIsMovable) || mOverlappedWithRobot;
	mOldX1 = event->scenePos().x() - x1();
	mOldY1 = event->scenePos().y() - y1();
}

void WallItem::mouseMoveEvent(QGraphicsSceneMouseEvent * event)
{
	if (!editable()) {
		return;
	}

	// When selecting a robot item on the scene than display widget may appear.
	// After that scene would be shrinked and mouse move event would be generated (but actually
	// mouse cursor is not moved). Because of that selected robot item moves which should not be because you
	// only select robot item and not move it. It also happens when you unselect robot item by selecting another
	// item (not only robot item). In this case new selected item moves.
	if (event->lastScreenPos() == event->screenPos()) {
		return;
	}

	const QRectF oldPos = QRectF(QPointF(x1(), y1()), QPointF(x2(), y2()));

	if (mDragged && ((flags() & ItemIsMovable) || mOverlappedWithRobot)) {
		const QPointF pos = event->scenePos();
		const qreal deltaX = (x1() - x2());
		const qreal deltaY = (y1() - y2());
		setX1(pos.x() - mOldX1);
		setY1(pos.y() - mOldY1);

		if (SettingsManager::value("2dShowGrid").toBool()) {
			const int indexGrid = SettingsManager::value("2dGridCellSize").toInt();
			reshapeBeginWithGrid(indexGrid);
			mCellNumbX1 = x1() / indexGrid;
			mCellNumbY1 = y1() / indexGrid;
			mCellNumbX2 = x2() / indexGrid;
			mCellNumbY2 = y2() / indexGrid;
		}

		setDraggedEnd(deltaX, deltaY);
	}  else if (mDragged) {
		QGraphicsItem::mouseMoveEvent(event);
	}

	// Items under cursor cannot be dragged when adding new item,
	// but it mustn`t confuse the case when item is unmovable
	// because overapped with robot
	if (mDragged && ((flags() & ItemIsMovable) || mOverlappedWithRobot)) {
		emit wallDragged(this, realShape(), oldPos);
	}
	event->accept();
}

bool WallItem::isDragged() const
{
	return mDragged;
}

qreal WallItem::width() const
{
	return pen().width();
}

void WallItem::mouseReleaseEvent(QGraphicsSceneMouseEvent * event)
{
	QGraphicsItem::mouseReleaseEvent(event);
	mDragged = false;
}

void WallItem::serialize(QDomElement &wallNode) const
{
	wallNode.setTagName("wall");
	AbstractItem::serialize(wallNode);
	mLineImpl.serialize(wallNode, x1() + scenePos().x(), y1() + scenePos().y()
			, x2() + scenePos().x(), y2() + scenePos().y());
}

void WallItem::deserialize(const QDomElement &element)
{
	AbstractItem::deserialize(element);
	const QPair<QPointF, QPointF> points = mLineImpl.deserialize(element);
	const QPointF begin = points.first;
	const QPointF end = points.second;

	setX1(begin.x());
	setY1(begin.y());
	setX2(end.x());
	setY2(end.y());

	recalculateBorders();
}

void WallItem::onOverlappedWithRobot(bool overlapped)
{
	mOverlappedWithRobot = overlapped;
}

QPainterPath WallItem::path() const
{
	return mPath;
}

void WallItem::recalculateBorders()
{
	QPainterPath wallPath;
	wallPath.moveTo(begin());

	if (mathUtils::Geometry::eq(begin(), end())) {
		wallPath.lineTo(end().x() + 0.1, end().y());
	} else {
		wallPath.lineTo(end());
	}

	QPainterPathStroker stroker;
	stroker.setWidth(wallWidth * 3 / 2);
	mPath = stroker.createStroke(wallPath);
}

void WallItem::resizeItem(QGraphicsSceneMouseEvent *event)
{
	if (event->modifiers() & Qt::ShiftModifier) {
		setX2(event->scenePos().x());
		setY2(event->scenePos().y());
		reshapeRectWithShift();
	} else {
		if (SettingsManager::value("2dShowGrid").toBool() && (dragState() == TopLeft || dragState() == BottomRight)) {
			resizeWithGrid(event, SettingsManager::value("2dGridCellSize").toInt());
		} else {
			if (dragState() == TopLeft || dragState() == BottomRight) {
				SolidItem::resizeItem(event);
			} else {
				setFlag(QGraphicsItem::ItemIsMovable, true);
			}
		}
	}
}

void WallItem::resizeWithGrid(QGraphicsSceneMouseEvent *event, int indexGrid)
{
	const qreal x = mapFromScene(event->scenePos()).x();
	const qreal y = mapFromScene(event->scenePos()).y();

	if (dragState() != None) {
		setFlag(QGraphicsItem::ItemIsMovable, false);
	}

	if (dragState() == TopLeft) {
		setX1(x);
		setY1(y);
		resizeBeginWithGrid(indexGrid);
	} else if (dragState() == BottomRight) {
		setX2(x);
		setY2(y);
		reshapeEndWithGrid(indexGrid);
	}
}

void WallItem::resizeBeginWithGrid(int indexGrid)
{
	const int coefX = static_cast<int>(x1()) / indexGrid;
	const int coefY = static_cast<int>(y1()) / indexGrid;

	setX1(alignedCoordinate(x1(), coefX, indexGrid));
	setY1(alignedCoordinate(y1(), coefY, indexGrid));

	mCellNumbX1 = x1() / indexGrid;
	mCellNumbY1 = y1() / indexGrid;
}

void WallItem::reshapeEndWithGrid(int indexGrid)
{
	const int coefX = static_cast<int>(x2()) / indexGrid;
	const int coefY = static_cast<int>(y2()) / indexGrid;

	setX2(alignedCoordinate(x2(), coefX, indexGrid));
	setY2(alignedCoordinate(y2(), coefY, indexGrid));

	mCellNumbX2 = x2() / indexGrid;
	mCellNumbY2 = y2() / indexGrid;
}

void WallItem::reshapeBeginWithGrid(int indexGrid)
{
	const int coefX = static_cast<int> (x1()) / indexGrid;
	const int coefY = static_cast<int> (y1()) / indexGrid;
	setX1(alignedCoordinate(x1(), coefX, indexGrid));
	setY1(alignedCoordinate(y1(), coefY, indexGrid));
	mCellNumbX1 = x1() / indexGrid;
	mCellNumbY1 = y1() / indexGrid;
}

void WallItem::alignTheWall(int indexGrid)
{
	if (x1() != x2() && y1() != y2()) {
		countCellNumbCoordinates(indexGrid);
	}

	setBeginCoordinatesWithGrid(indexGrid);
	setEndCoordinatesWithGrid(indexGrid);
}

void WallItem::countCellNumbCoordinates(int indexGrid)
{
	mCellNumbX1 = x1() / indexGrid;
	mCellNumbY1 = y1() / indexGrid;

	if (qAbs(y2() - y1()) > qAbs(x2() - x1())) {
		mCellNumbX2 = mCellNumbX1;
		mCellNumbY2 = y2() / indexGrid;
	} else {
		mCellNumbX2 = x2() / indexGrid;
		mCellNumbY2 = mCellNumbY1;
	}
}

void WallItem::setBeginCoordinatesWithGrid(int indexGrid)
{
	setX1(mCellNumbX1 * indexGrid);
	setY1(mCellNumbY1 * indexGrid);
}

void WallItem::setEndCoordinatesWithGrid(int indexGrid)
{
	setX2(mCellNumbX2 * indexGrid);
	setY2(mCellNumbY2 * indexGrid);
}

void WallItem::setDraggedEnd(qreal x, qreal y)
{
	setX2(x1() - x);
	setY2(y1() - y);
}

qreal WallItem::alignedCoordinate(qreal coord, int coef, const int indexGrid) const
{
	const int coefSign = coef ? coef / qAbs(coef) : 0;

	if (qAbs(qAbs(coord) - qAbs(coef) * indexGrid) <= indexGrid / 2) {
		return coef * indexGrid;
	} else if (qAbs(qAbs(coord) - (qAbs(coef) + 1) * indexGrid) <= indexGrid / 2) {
		return (coef + coefSign) * indexGrid;
	}

	return coord;
}
<commit_msg>Fixed incorrent grid alignment of null robot walls<commit_after>/* Copyright 2007-2015 QReal Research Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. */

#include "wallItem.h"

#include <QtWidgets/QAction>
#include <QtWidgets/QGraphicsSceneMouseEvent>

#include <qrkernel/settingsManager.h>
#include <qrutils/mathUtils/geometry.h>

using namespace twoDModel::items;
using namespace qReal;
using namespace graphicsUtils;

const int wallWidth = 10;

WallItem::WallItem(const QPointF &begin, const QPointF &end)
	: SolidItem()
	, mDragged(false)
	, mImage(":/icons/2d_wall.png")
	, mOldX1(0)
	, mOldY1(0)
	, mCellNumbX1(0)
	, mCellNumbY1(0)
	, mCellNumbX2(0)
	, mCellNumbY2(0)
{
	setX1(begin.x());
	setY1(begin.y());
	setX2(end.x());
	setY2(end.y());
	setFlags(ItemIsSelectable | ItemIsMovable);
	setPrivateData();
	setAcceptDrops(true);
}

WallItem *WallItem::clone() const
{
	WallItem * const cloned = new WallItem({x1(), y1()}, {x2(), y2()});
	AbstractItem::copyTo(cloned);
	connect(this, &AbstractItem::positionChanged, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::x1Changed, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::y1Changed, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::x2Changed, cloned, &WallItem::recalculateBorders);
	connect(this, &AbstractItem::y2Changed, cloned, &WallItem::recalculateBorders);

	cloned->mCellNumbX1 = mCellNumbX1;
	cloned->mCellNumbY1 = mCellNumbY1;
	cloned->mCellNumbX2 = mCellNumbX2;
	cloned->mCellNumbY2 = mCellNumbY2;

	cloned->mOldX1 = mOldX1;
	cloned->mOldY1 = mOldY1;
	cloned->mDragged = mDragged;
	cloned->mOverlappedWithRobot = mOverlappedWithRobot;
	cloned->mPath = mPath;
	return cloned;
}

QAction *WallItem::wallTool()
{
	QAction * const result = new QAction(QIcon(":/icons/2d_wall.png"), tr("Wall (W)"), nullptr);
	result->setShortcut(QKeySequence(Qt::Key_W));
	return result;
}

void WallItem::setPrivateData()
{
	setZValue(1);
	QPen pen(this->pen());
	pen.setWidth(wallWidth);
	pen.setStyle(Qt::NoPen);
	setPen(pen);
	QBrush brush(this->brush());
	brush.setStyle(Qt::SolidPattern);
	brush.setTextureImage(mImage);
	setBrush(brush);
}

QPointF WallItem::begin() const
{
	return QPointF(x1(), y1()) + scenePos();
}

QPointF WallItem::end() const
{
	return QPointF(x2(), y2()) + scenePos();
}

QRectF WallItem::boundingRect() const
{
	return mLineImpl.boundingRect(x1(), y1(), x2(), y2(), pen().width(), drift);
}

QPainterPath WallItem::shape() const
{
	return mLineImpl.shape(drift, x1(), y1(), x2(), y2());
}

void WallItem::drawItem(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
	Q_UNUSED(option);
	Q_UNUSED(widget);
	painter->drawPath(shape());
	recalculateBorders();
}

void WallItem::drawExtractionForItem(QPainter *painter)
{
	if (!isSelected()) {
		return;
	}

	painter->setPen(QPen(Qt::green));
	mLineImpl.drawExtractionForItem(painter, x1(), y1(), x2(), y2(), drift);
	mLineImpl.drawFieldForResizeItem(painter, resizeDrift, x1(), y1(), x2(), y2());
}

void WallItem::mousePressEvent(QGraphicsSceneMouseEvent * event)
{
	AbstractItem::mousePressEvent(event);

	if (!editable()) {
		return;
	}

	mDragged = (flags() & ItemIsMovable) || mOverlappedWithRobot;
	mOldX1 = event->scenePos().x() - x1();
	mOldY1 = event->scenePos().y() - y1();
}

void WallItem::mouseMoveEvent(QGraphicsSceneMouseEvent * event)
{
	if (!editable()) {
		return;
	}

	// When selecting a robot item on the scene than display widget may appear.
	// After that scene would be shrinked and mouse move event would be generated (but actually
	// mouse cursor is not moved). Because of that selected robot item moves which should not be because you
	// only select robot item and not move it. It also happens when you unselect robot item by selecting another
	// item (not only robot item). In this case new selected item moves.
	if (event->lastScreenPos() == event->screenPos()) {
		return;
	}

	const QRectF oldPos = QRectF(QPointF(x1(), y1()), QPointF(x2(), y2()));

	if (mDragged && ((flags() & ItemIsMovable) || mOverlappedWithRobot)) {
		const QPointF pos = event->scenePos();
		const qreal deltaX = (x1() - x2());
		const qreal deltaY = (y1() - y2());
		setX1(pos.x() - mOldX1);
		setY1(pos.y() - mOldY1);

		if (SettingsManager::value("2dShowGrid").toBool()) {
			const int indexGrid = SettingsManager::value("2dGridCellSize").toInt();
			reshapeBeginWithGrid(indexGrid);
			mCellNumbX1 = x1() / indexGrid;
			mCellNumbY1 = y1() / indexGrid;
			mCellNumbX2 = x2() / indexGrid;
			mCellNumbY2 = y2() / indexGrid;
		}

		setDraggedEnd(deltaX, deltaY);
	}  else if (mDragged) {
		QGraphicsItem::mouseMoveEvent(event);
	}

	// Items under cursor cannot be dragged when adding new item,
	// but it mustn`t confuse the case when item is unmovable
	// because overapped with robot
	if (mDragged && ((flags() & ItemIsMovable) || mOverlappedWithRobot)) {
		emit wallDragged(this, realShape(), oldPos);
	}
	event->accept();
}

bool WallItem::isDragged() const
{
	return mDragged;
}

qreal WallItem::width() const
{
	return pen().width();
}

void WallItem::mouseReleaseEvent(QGraphicsSceneMouseEvent * event)
{
	QGraphicsItem::mouseReleaseEvent(event);
	mDragged = false;
}

void WallItem::serialize(QDomElement &wallNode) const
{
	wallNode.setTagName("wall");
	AbstractItem::serialize(wallNode);
	mLineImpl.serialize(wallNode, x1() + scenePos().x(), y1() + scenePos().y()
			, x2() + scenePos().x(), y2() + scenePos().y());
}

void WallItem::deserialize(const QDomElement &element)
{
	AbstractItem::deserialize(element);
	const QPair<QPointF, QPointF> points = mLineImpl.deserialize(element);
	const QPointF begin = points.first;
	const QPointF end = points.second;

	setX1(begin.x());
	setY1(begin.y());
	setX2(end.x());
	setY2(end.y());

	recalculateBorders();
}

void WallItem::onOverlappedWithRobot(bool overlapped)
{
	mOverlappedWithRobot = overlapped;
}

QPainterPath WallItem::path() const
{
	return mPath;
}

void WallItem::recalculateBorders()
{
	QPainterPath wallPath;
	wallPath.moveTo(begin());

	if (mathUtils::Geometry::eq(begin(), end())) {
		wallPath.lineTo(end().x() + 0.1, end().y());
	} else {
		wallPath.lineTo(end());
	}

	QPainterPathStroker stroker;
	stroker.setWidth(wallWidth * 3 / 2);
	mPath = stroker.createStroke(wallPath);
}

void WallItem::resizeItem(QGraphicsSceneMouseEvent *event)
{
	if (event->modifiers() & Qt::ShiftModifier) {
		setX2(event->scenePos().x());
		setY2(event->scenePos().y());
		reshapeRectWithShift();
	} else {
		if (SettingsManager::value("2dShowGrid").toBool() && (dragState() == TopLeft || dragState() == BottomRight)) {
			resizeWithGrid(event, SettingsManager::value("2dGridCellSize").toInt());
		} else {
			if (dragState() == TopLeft || dragState() == BottomRight) {
				SolidItem::resizeItem(event);
			} else {
				setFlag(QGraphicsItem::ItemIsMovable, true);
			}
		}
	}
}

void WallItem::resizeWithGrid(QGraphicsSceneMouseEvent *event, int indexGrid)
{
	const qreal x = mapFromScene(event->scenePos()).x();
	const qreal y = mapFromScene(event->scenePos()).y();

	if (dragState() != None) {
		setFlag(QGraphicsItem::ItemIsMovable, false);
	}

	if (dragState() == TopLeft) {
		setX1(x);
		setY1(y);
		resizeBeginWithGrid(indexGrid);
	} else if (dragState() == BottomRight) {
		setX2(x);
		setY2(y);
		reshapeEndWithGrid(indexGrid);
	}
}

void WallItem::resizeBeginWithGrid(int indexGrid)
{
	const int coefX = static_cast<int>(x1()) / indexGrid;
	const int coefY = static_cast<int>(y1()) / indexGrid;

	setX1(alignedCoordinate(x1(), coefX, indexGrid));
	setY1(alignedCoordinate(y1(), coefY, indexGrid));

	mCellNumbX1 = x1() / indexGrid;
	mCellNumbY1 = y1() / indexGrid;
}

void WallItem::reshapeEndWithGrid(int indexGrid)
{
	const int coefX = static_cast<int>(x2()) / indexGrid;
	const int coefY = static_cast<int>(y2()) / indexGrid;

	setX2(alignedCoordinate(x2(), coefX, indexGrid));
	setY2(alignedCoordinate(y2(), coefY, indexGrid));

	mCellNumbX2 = x2() / indexGrid;
	mCellNumbY2 = y2() / indexGrid;
}

void WallItem::reshapeBeginWithGrid(int indexGrid)
{
	const int coefX = static_cast<int> (x1()) / indexGrid;
	const int coefY = static_cast<int> (y1()) / indexGrid;
	setX1(alignedCoordinate(x1(), coefX, indexGrid));
	setY1(alignedCoordinate(y1(), coefY, indexGrid));
	mCellNumbX1 = x1() / indexGrid;
	mCellNumbY1 = y1() / indexGrid;
}

void WallItem::alignTheWall(int indexGrid)
{
	countCellNumbCoordinates(indexGrid);
	setBeginCoordinatesWithGrid(indexGrid);
	setEndCoordinatesWithGrid(indexGrid);
}

void WallItem::countCellNumbCoordinates(int indexGrid)
{
	mCellNumbX1 = x1() / indexGrid;
	mCellNumbY1 = y1() / indexGrid;

	if (qAbs(y2() - y1()) > qAbs(x2() - x1())) {
		mCellNumbX2 = mCellNumbX1;
		mCellNumbY2 = y2() / indexGrid;
	} else {
		mCellNumbX2 = x2() / indexGrid;
		mCellNumbY2 = mCellNumbY1;
	}
}

void WallItem::setBeginCoordinatesWithGrid(int indexGrid)
{
	setX1(mCellNumbX1 * indexGrid);
	setY1(mCellNumbY1 * indexGrid);
}

void WallItem::setEndCoordinatesWithGrid(int indexGrid)
{
	setX2(mCellNumbX2 * indexGrid);
	setY2(mCellNumbY2 * indexGrid);
}

void WallItem::setDraggedEnd(qreal x, qreal y)
{
	setX2(x1() - x);
	setY2(y1() - y);
}

qreal WallItem::alignedCoordinate(qreal coord, int coef, const int indexGrid) const
{
	const int coefSign = coef ? coef / qAbs(coef) : 0;

	if (qAbs(qAbs(coord) - qAbs(coef) * indexGrid) <= indexGrid / 2) {
		return coef * indexGrid;
	} else if (qAbs(qAbs(coord) - (qAbs(coef) + 1) * indexGrid) <= indexGrid / 2) {
		return (coef + coefSign) * indexGrid;
	}

	return coord;
}
<|endoftext|>


<commit_before>/**
 * Copyright (c) 2015 - The CM Authors <legal@clickmatcher.com>
 *   All Rights Reserved.
 *
 * This file is CONFIDENTIAL -- Distribution or duplication of this material or
 * the information contained herein is strictly forbidden unless prior written
 * permission is obtained.
 */
#include <thread>
#include "CTRByPositionServlet.h"
#include "CTRCounter.h"
#include "fnord-base/Language.h"
#include "fnord-base/logging.h"
#include "fnord-base/wallclock.h"
#include "fnord-base/io/fileutil.h"
#include "analytics/CTRByPositionQuery.h"

using namespace fnord;

namespace cm {

CTRByPositionServlet::CTRByPositionServlet(VFS* vfs) : vfs_(vfs) {}

void CTRByPositionServlet::handleHTTPRequest(
    http::HTTPRequest* req,
    http::HTTPResponse* res) {
  URI uri(req->uri());
  const auto& params = uri.queryParams();

  auto end_time = WallClock::unixMicros();
  auto start_time = end_time - 30 * kMicrosPerDay;
  Set<String> test_groups;
  Set<String> device_types;

  /* arguments */
  String customer;
  if (!URI::getParam(params, "customer", &customer)) {
    res->addBody("error: missing ?customer=... parameter");
    res->setStatus(http::kStatusBadRequest);
    return;
  }

  String lang_str;
  if (!URI::getParam(params, "lang", &lang_str)) {
    res->addBody("error: missing ?lang=... parameter");
    res->setStatus(http::kStatusBadRequest);
    return;
  }

  for (const auto& p : params) {
    if (p.first == "from") {
      start_time = std::stoul(p.second) * kMicrosPerSecond;
      continue;
    }

    if (p.first == "until") {
      end_time = std::stoul(p.second) * kMicrosPerSecond;
      continue;
    }

    if (p.first == "test_group") {
      test_groups.emplace(p.second);
      continue;
    }

    if (p.first == "device_type") {
      device_types.emplace(p.second);
      continue;
    }

  }

  if (test_groups.size() == 0) {
    test_groups.emplace("all");
  }

  if (device_types.size() == 0) {
    device_types = Set<String> { "unknown", "desktop", "tablet", "phone" };
  }

  /* execute query*/
  auto t0 = WallClock::unixMicros();

  cm::CTRByPositionQueryResult result;
  std::mutex mutex;
  size_t num_threads;
  std::condition_variable cv;

  for (uint64_t i = end_time; i >= start_time; i -= kMicrosPerHour * 4) {
    auto table_file = StringUtil::format(
        "/srv/cmdata/artifacts/$0_joined_sessions.$1.cstable",
        customer,
        i / (kMicrosPerHour * 4));

    if (!FileUtil::exists(table_file)) {
      fnord::logWarning(
          "cm.ctrbypositionservlet",
          "missing table: $0",
          table_file);

      continue;
    }

    std::unique_lock<std::mutex> lk(mutex);
    while (num_threads >= 8) {
      cv.wait(lk);
    }

    auto t = std::thread([table_file, &mutex, &result, &num_threads, &cv] () {
      cstable::CSTableReader reader(table_file);
      cm::AnalyticsQuery aq;
      cm::CTRByPositionQueryResult r;
      cm::CTRByPositionQuery q(&aq, &r);
      aq.scanTable(&reader);

      std::unique_lock<std::mutex> lk(mutex);
      result.merge(r);
      --num_threads;
      lk.unlock();
      cv.notify_one();
    });

    t.detach();
  }

  uint64_t total_views = 0;
  uint64_t total_clicks = 0;

  for (const auto& c : result.counters) {
    total_views += c.second.num_views;
    total_clicks += c.second.num_clicks;
  }

  auto t1 = WallClock::unixMicros();

  /* write response */
  res->setStatus(http::kStatusOK);
  res->addHeader("Content-Type", "application/json; charset=utf-8");
  json::JSONOutputStream json(res->getBodyOutputStream());

  json.beginObject();
  json.addObjectEntry("rows_scanned");
  json.addInteger(result.rows_scanned);
  json.addComma();
  json.addObjectEntry("execution_time_ms");
  json.addFloat((t1 - t0) / 1000.0f);
  json.addComma();
  json.addObjectEntry("results");
  json.beginArray();

  int n = 0;
  for (const auto& c : result.counters) {
    if (++n > 1) {
      json.addComma();
    }

    json.beginObject();
    json.addObjectEntry("position");
    json.addInteger(c.first);
    json.addComma();
    json.addObjectEntry("views");
    json.addInteger(c.second.num_views);
    json.addComma();
    json.addObjectEntry("clicks");
    json.addInteger(c.second.num_clicks);
    json.addComma();
    json.addObjectEntry("ctr");
    json.addFloat(c.second.num_clicks / (double) c.second.num_views);
    json.addComma();
    json.addObjectEntry("ctr_base");
    json.addFloat(c.second.num_clicks / (double) total_views);
    json.addComma();
    json.addObjectEntry("clickshare");
    json.addFloat(c.second.num_clicks / (double) total_clicks);
    json.endObject();
  }

  json.endArray();
  json.endObject();
}

}
<commit_msg>multi-threaded query execution<commit_after>/**
 * Copyright (c) 2015 - The CM Authors <legal@clickmatcher.com>
 *   All Rights Reserved.
 *
 * This file is CONFIDENTIAL -- Distribution or duplication of this material or
 * the information contained herein is strictly forbidden unless prior written
 * permission is obtained.
 */
#include <thread>
#include "CTRByPositionServlet.h"
#include "CTRCounter.h"
#include "fnord-base/Language.h"
#include "fnord-base/logging.h"
#include "fnord-base/wallclock.h"
#include "fnord-base/io/fileutil.h"
#include "analytics/CTRByPositionQuery.h"

using namespace fnord;

namespace cm {

CTRByPositionServlet::CTRByPositionServlet(VFS* vfs) : vfs_(vfs) {}

void CTRByPositionServlet::handleHTTPRequest(
    http::HTTPRequest* req,
    http::HTTPResponse* res) {
  URI uri(req->uri());
  const auto& params = uri.queryParams();

  auto end_time = WallClock::unixMicros();
  auto start_time = end_time - 30 * kMicrosPerDay;
  Set<String> test_groups;
  Set<String> device_types;

  /* arguments */
  String customer;
  if (!URI::getParam(params, "customer", &customer)) {
    res->addBody("error: missing ?customer=... parameter");
    res->setStatus(http::kStatusBadRequest);
    return;
  }

  String lang_str;
  if (!URI::getParam(params, "lang", &lang_str)) {
    res->addBody("error: missing ?lang=... parameter");
    res->setStatus(http::kStatusBadRequest);
    return;
  }

  for (const auto& p : params) {
    if (p.first == "from") {
      start_time = std::stoul(p.second) * kMicrosPerSecond;
      continue;
    }

    if (p.first == "until") {
      end_time = std::stoul(p.second) * kMicrosPerSecond;
      continue;
    }

    if (p.first == "test_group") {
      test_groups.emplace(p.second);
      continue;
    }

    if (p.first == "device_type") {
      device_types.emplace(p.second);
      continue;
    }

  }

  if (test_groups.size() == 0) {
    test_groups.emplace("all");
  }

  if (device_types.size() == 0) {
    device_types = Set<String> { "unknown", "desktop", "tablet", "phone" };
  }

  /* execute query*/
  auto t0 = WallClock::unixMicros();

  cm::CTRByPositionQueryResult result;
  std::mutex mutex;
  size_t num_threads;
  std::condition_variable cv;

  for (uint64_t i = end_time; i >= start_time; i -= kMicrosPerHour * 4) {
    auto table_file = StringUtil::format(
        "/srv/cmdata/artifacts/$0_joined_sessions.$1.cstable",
        customer,
        i / (kMicrosPerHour * 4));

    if (!FileUtil::exists(table_file)) {
      fnord::logWarning(
          "cm.ctrbypositionservlet",
          "missing table: $0",
          table_file);

      continue;
    }

    std::unique_lock<std::mutex> lk(mutex);
    while (num_threads >= 8) {
      cv.wait(lk);
    }

    ++num_threads;
    auto t = std::thread([table_file, &mutex, &result, &num_threads, &cv] () {
      cstable::CSTableReader reader(table_file);
      cm::AnalyticsQuery aq;
      cm::CTRByPositionQueryResult r;
      cm::CTRByPositionQuery q(&aq, &r);
      aq.scanTable(&reader);

      std::unique_lock<std::mutex> lk(mutex);
      result.merge(r);
      --num_threads;
      lk.unlock();
      cv.notify_one();
    });

    t.detach();
  }

  while (num_threads > 0) {
    cv.wait(lk);
  }

  uint64_t total_views = 0;
  uint64_t total_clicks = 0;

  for (const auto& c : result.counters) {
    total_views += c.second.num_views;
    total_clicks += c.second.num_clicks;
  }

  auto t1 = WallClock::unixMicros();

  /* write response */
  res->setStatus(http::kStatusOK);
  res->addHeader("Content-Type", "application/json; charset=utf-8");
  json::JSONOutputStream json(res->getBodyOutputStream());

  json.beginObject();
  json.addObjectEntry("rows_scanned");
  json.addInteger(result.rows_scanned);
  json.addComma();
  json.addObjectEntry("execution_time_ms");
  json.addFloat((t1 - t0) / 1000.0f);
  json.addComma();
  json.addObjectEntry("results");
  json.beginArray();

  int n = 0;
  for (const auto& c : result.counters) {
    if (++n > 1) {
      json.addComma();
    }

    json.beginObject();
    json.addObjectEntry("position");
    json.addInteger(c.first);
    json.addComma();
    json.addObjectEntry("views");
    json.addInteger(c.second.num_views);
    json.addComma();
    json.addObjectEntry("clicks");
    json.addInteger(c.second.num_clicks);
    json.addComma();
    json.addObjectEntry("ctr");
    json.addFloat(c.second.num_clicks / (double) c.second.num_views);
    json.addComma();
    json.addObjectEntry("ctr_base");
    json.addFloat(c.second.num_clicks / (double) total_views);
    json.addComma();
    json.addObjectEntry("clickshare");
    json.addFloat(c.second.num_clicks / (double) total_clicks);
    json.endObject();
  }

  json.endArray();
  json.endObject();
}

}
<|endoftext|>


<commit_before>#include <htl_stdinc.hpp>

// system
#include <unistd.h>
#include <errno.h>
#include <stdlib.h>
#include <sys/time.h>
#include <inttypes.h>

// socket
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

// dns
#include <netdb.h>

#include <string>
using namespace std;

#include <htl_core_error.hpp>
#include <htl_core_log.hpp>

#include <htl_os_st.hpp>

StStatistic::StStatistic(){
    starttime = StUtility::GetCurrentTime();
    task_duration = 0;
    threads = alive = 0;
    nread = nwrite = 0;
    tasks = err_tasks = sub_tasks = err_sub_tasks = 0;
}

StStatistic::~StStatistic(){
}

void StStatistic::OnRead(int /*tid*/, ssize_t nread_bytes){
    this->nread += nread_bytes;
}

void StStatistic::OnWrite(int /*tid*/, ssize_t nwrite_bytes){
    this->nwrite += nwrite_bytes;
}

void StStatistic::OnThreadRun(int /*tid*/){
    threads++;
}

void StStatistic::OnThreadQuit(int /*tid*/){
    threads--;
}

void StStatistic::OnTaskStart(int /*tid*/, std::string /*task_url*/){
    alive++;
    tasks++;
}

void StStatistic::OnTaskError(int /*tid*/, int duration_seconds){
    alive--;
    err_tasks++;
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::OnTaskEnd(int /*tid*/, int duration_seconds){
    alive--;
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::OnSubTaskStart(int /*tid*/, std::string /*sub_task_url*/){
    sub_tasks++;
}

void StStatistic::OnSubTaskError(int /*tid*/, int duration_seconds){
    err_sub_tasks++;
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::OnSubTaskEnd(int /*tid*/, int duration_seconds){
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::DoReport(double sleep_ms){
    for(;;){
        int64_t duration = StUtility::GetCurrentTime() - starttime;
        double read_mbps = 0, write_mbps = 0;

        if(duration > 0){
            read_mbps = nread * 8.0 / duration / 1000;
            write_mbps = nwrite * 8.0 / duration / 1000;
        }

        double avarage_duration = task_duration/1000.0;
        if(tasks > 0){
            avarage_duration /= tasks;
        }
        LReport("[report] [%d] threads:%d alive:%d duration:%.0f tduration:%.0f nread:%.2f nwrite:%.2f "
            "tasks:%"PRId64" etasks:%"PRId64" stasks:%"PRId64" estasks:%"PRId64,
            getpid(), threads, alive, duration/1000.0, avarage_duration, read_mbps, write_mbps,
            tasks, err_tasks, sub_tasks, err_sub_tasks);

        st_usleep(sleep_ms * 1000);
    }
}

StStatistic* statistic = new StStatistic();

StTask::StTask(){
    static int _id = 0;
    id = ++_id;
}

StTask::~StTask(){
}

int StTask::GetId(){
    return id;
}

StFarm::StFarm(){
}

StFarm::~StFarm(){
}

int StFarm::Initialize(double report){
    int ret = ERROR_SUCCESS;

    report_seconds = report;

    // use linux epoll.
    if(st_set_eventsys(ST_EVENTSYS_ALT) == -1){
        ret = ERROR_ST_INITIALIZE;
        Error("st_set_eventsys use linux epoll failed. ret=%d", ret);
        return ret;
    }

    if(st_init() != 0){
        ret = ERROR_ST_INITIALIZE;
        Error("st_init failed. ret=%d", ret);
        return ret;
    }

    StUtility::InitRandom();

    return ret;
}

int StFarm::Spawn(StTask* task){
    int ret = ERROR_SUCCESS;

    if(st_thread_create(st_thread_function, task, 0, 0) == NULL){
        ret = ERROR_ST_THREAD_CREATE;
        Error("crate st_thread failed, ret=%d", ret);
        return ret;
    }

    Trace("create thread for task #%d success", task->GetId());

    return ret;
}

int StFarm::WaitAll(){
    int ret = ERROR_SUCCESS;

    // main thread turn to a report therad.
    statistic->DoReport(report_seconds * 1000);

    st_thread_exit(NULL);

    return ret;
}

void* StFarm::st_thread_function(void* args){
    StTask* task = (StTask*)args;

    context->SetId(task->GetId());

    statistic->OnThreadRun(task->GetId());

    int ret = task->Process();

    statistic->OnThreadQuit(task->GetId());

    if(ret != ERROR_SUCCESS){
        Warn("st task terminate with ret=%d", ret);
    }
    else{
        Trace("st task terminate with ret=%d", ret);
    }

    delete task;

    return NULL;
}

StSocket::StSocket(){
    sock_nfd = NULL;
    status = SocketInit;
}

StSocket::~StSocket(){
    Close();
}

SocketStatus StSocket::Status(){
    return status;
}

int StSocket::Connect(const char* ip, int port){
    int ret = ERROR_SUCCESS;

    Close();

    int sock = socket(AF_INET, SOCK_STREAM, 0);
    if(sock == -1){
        ret = ERROR_SOCKET;
        Error("create socket error. ret=%d", ret);
        return ret;
    }

    int reuse_socket = 1;
    if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuse_socket, sizeof(int)) == -1){
        ret = ERROR_SOCKET;
        Error("setsockopt reuse-addr error. ret=%d", ret);
        return ret;
    }

    sock_nfd = st_netfd_open_socket(sock);
    if(sock_nfd == NULL){
        ret = ERROR_OPEN_SOCKET;
        Error("st_netfd_open_socket failed. ret=%d", ret);
        return ret;
    }
    Info("create socket(%d) success", sock);

    // connect to server
    sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);
    addr.sin_addr.s_addr = inet_addr(ip);

    if(st_connect(sock_nfd, (const struct sockaddr*)&addr, sizeof(sockaddr_in), ST_UTIME_NO_TIMEOUT) == -1){
        ret = ERROR_CONNECT;
        Error("connect to server(%s:%d) error. ret=%d", ip, port, ret);
        return ret;
    }
    Info("connec to server %s at port %d success", ip, port);

    status = SocketConnected;

    return ret;
}

int StSocket::Read(const void* buf, size_t size, ssize_t* nread){
    int ret = ERROR_SUCCESS;

    *nread = st_read(sock_nfd, (void*)buf, size, ST_UTIME_NO_TIMEOUT);

    if(*nread <= 0){
        if(errno == ETIME){
            errno = EAGAIN;
        }

        ret = ERROR_READ;
        status = SocketDisconnected;
    }

    if(*nread > 0){
        statistic->OnRead(context->GetId(), *nread);
    }

    return ret;
}

int StSocket::Write(const void* buf, size_t size, ssize_t* nwrite){
    int ret = ERROR_SUCCESS;

    *nwrite = st_write(sock_nfd, (void*)buf, size, ST_UTIME_NO_TIMEOUT);

    if(*nwrite <= 0){
        if(errno == ETIME){
            errno = EAGAIN;
        }

        ret = ERROR_SEND;
        status = SocketDisconnected;
    }

    if(*nwrite > 0){
        statistic->OnWrite(context->GetId(), *nwrite);
    }

    return ret;
}

int StSocket::Close(){
    int ret = ERROR_SUCCESS;

    if(sock_nfd == NULL){
        return ret;
    }

    if(st_netfd_close(sock_nfd) != 0){
        ret = ERROR_CLOSE;
    }

    sock_nfd = NULL;
    status = SocketDisconnected;

    return ret;
}

int64_t StUtility::GetCurrentTime(){
    timeval now;

    int ret = gettimeofday(&now, NULL);

    if(ret == -1){
        Warn("gettimeofday error, ret=%d", ret);
    }

    // we must convert the tv_sec/tv_usec to int64_t.
    return ((int64_t)now.tv_sec)*1000 + ((int64_t)now.tv_usec) / 1000;
}

void StUtility::InitRandom(){
    timeval now;

    if(gettimeofday(&now, NULL) == -1){
        srand(0);
        return;
    }

    srand(now.tv_sec * 1000000 + now.tv_usec);
}

st_utime_t StUtility::BuildRandomMTime(double sleep_seconds){
    if(sleep_seconds <= 0){
        return 0 * 1000;
    }

    // 80% consts value.
    // 40% random value.
    // to get more graceful random time to mocking HLS client.
    st_utime_t sleep_ms = (int)(sleep_seconds * 1000 * 0.7) + rand() % (int)(sleep_seconds * 1000 * 0.4);

    return sleep_ms;
}

int StUtility::DnsResolve(string host, string& ip){
    int ret = ERROR_SUCCESS;

    if(inet_addr(host.c_str()) != INADDR_NONE){
        ip = host;
        Info("dns resolve %s to %s", host.c_str(), ip.c_str());

        return ret;
    }

    hostent* answer = gethostbyname(host.c_str());
    if(answer == NULL){
        ret = ERROR_DNS_RESOLVE;
        Error("dns resolve host %s error. ret=%d", host.c_str(), ret);
        return ret;
    }

    char ipv4[16];
    memset(ipv4, 0, sizeof(ipv4));
    for(int i = 0; i < answer->h_length; i++){
        inet_ntop(AF_INET, answer->h_addr_list[i], ipv4, sizeof(ipv4));
        Info("dns resolve host %s to %s.", host.c_str(), ipv4);
        break;
    }

    ip = ipv4;
    Info("dns resolve %s to %s", host.c_str(), ip.c_str());

    return ret;
}

StLogContext::StLogContext(){
}

StLogContext::~StLogContext(){
}

void StLogContext::SetId(int id){
    cache[st_thread_self()] = id;
}

int StLogContext::GetId(){
    return cache[st_thread_self()];
}
<commit_msg>fix the errno of st_read.<commit_after>#include <htl_stdinc.hpp>

// system
#include <unistd.h>
#include <errno.h>
#include <stdlib.h>
#include <sys/time.h>
#include <inttypes.h>

// socket
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

// dns
#include <netdb.h>

#include <string>
using namespace std;

#include <htl_core_error.hpp>
#include <htl_core_log.hpp>

#include <htl_os_st.hpp>

StStatistic::StStatistic(){
    starttime = StUtility::GetCurrentTime();
    task_duration = 0;
    threads = alive = 0;
    nread = nwrite = 0;
    tasks = err_tasks = sub_tasks = err_sub_tasks = 0;
}

StStatistic::~StStatistic(){
}

void StStatistic::OnRead(int /*tid*/, ssize_t nread_bytes){
    this->nread += nread_bytes;
}

void StStatistic::OnWrite(int /*tid*/, ssize_t nwrite_bytes){
    this->nwrite += nwrite_bytes;
}

void StStatistic::OnThreadRun(int /*tid*/){
    threads++;
}

void StStatistic::OnThreadQuit(int /*tid*/){
    threads--;
}

void StStatistic::OnTaskStart(int /*tid*/, std::string /*task_url*/){
    alive++;
    tasks++;
}

void StStatistic::OnTaskError(int /*tid*/, int duration_seconds){
    alive--;
    err_tasks++;
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::OnTaskEnd(int /*tid*/, int duration_seconds){
    alive--;
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::OnSubTaskStart(int /*tid*/, std::string /*sub_task_url*/){
    sub_tasks++;
}

void StStatistic::OnSubTaskError(int /*tid*/, int duration_seconds){
    err_sub_tasks++;
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::OnSubTaskEnd(int /*tid*/, int duration_seconds){
    this->task_duration += duration_seconds * 1000;
}

void StStatistic::DoReport(double sleep_ms){
    for(;;){
        int64_t duration = StUtility::GetCurrentTime() - starttime;
        double read_mbps = 0, write_mbps = 0;

        if(duration > 0){
            read_mbps = nread * 8.0 / duration / 1000;
            write_mbps = nwrite * 8.0 / duration / 1000;
        }

        double avarage_duration = task_duration/1000.0;
        if(tasks > 0){
            avarage_duration /= tasks;
        }
        LReport("[report] [%d] threads:%d alive:%d duration:%.0f tduration:%.0f nread:%.2f nwrite:%.2f "
            "tasks:%"PRId64" etasks:%"PRId64" stasks:%"PRId64" estasks:%"PRId64,
            getpid(), threads, alive, duration/1000.0, avarage_duration, read_mbps, write_mbps,
            tasks, err_tasks, sub_tasks, err_sub_tasks);

        st_usleep(sleep_ms * 1000);
    }
}

StStatistic* statistic = new StStatistic();

StTask::StTask(){
    static int _id = 0;
    id = ++_id;
}

StTask::~StTask(){
}

int StTask::GetId(){
    return id;
}

StFarm::StFarm(){
}

StFarm::~StFarm(){
}

int StFarm::Initialize(double report){
    int ret = ERROR_SUCCESS;

    report_seconds = report;

    // use linux epoll.
    if(st_set_eventsys(ST_EVENTSYS_ALT) == -1){
        ret = ERROR_ST_INITIALIZE;
        Error("st_set_eventsys use linux epoll failed. ret=%d", ret);
        return ret;
    }

    if(st_init() != 0){
        ret = ERROR_ST_INITIALIZE;
        Error("st_init failed. ret=%d", ret);
        return ret;
    }

    StUtility::InitRandom();

    return ret;
}

int StFarm::Spawn(StTask* task){
    int ret = ERROR_SUCCESS;

    if(st_thread_create(st_thread_function, task, 0, 0) == NULL){
        ret = ERROR_ST_THREAD_CREATE;
        Error("crate st_thread failed, ret=%d", ret);
        return ret;
    }

    Trace("create thread for task #%d success", task->GetId());

    return ret;
}

int StFarm::WaitAll(){
    int ret = ERROR_SUCCESS;

    // main thread turn to a report therad.
    statistic->DoReport(report_seconds * 1000);

    st_thread_exit(NULL);

    return ret;
}

void* StFarm::st_thread_function(void* args){
    StTask* task = (StTask*)args;

    context->SetId(task->GetId());

    statistic->OnThreadRun(task->GetId());

    int ret = task->Process();

    statistic->OnThreadQuit(task->GetId());

    if(ret != ERROR_SUCCESS){
        Warn("st task terminate with ret=%d", ret);
    }
    else{
        Trace("st task terminate with ret=%d", ret);
    }

    delete task;

    return NULL;
}

StSocket::StSocket(){
    sock_nfd = NULL;
    status = SocketInit;
}

StSocket::~StSocket(){
    Close();
}

SocketStatus StSocket::Status(){
    return status;
}

int StSocket::Connect(const char* ip, int port){
    int ret = ERROR_SUCCESS;

    Close();

    int sock = socket(AF_INET, SOCK_STREAM, 0);
    if(sock == -1){
        ret = ERROR_SOCKET;
        Error("create socket error. ret=%d", ret);
        return ret;
    }

    int reuse_socket = 1;
    if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuse_socket, sizeof(int)) == -1){
        ret = ERROR_SOCKET;
        Error("setsockopt reuse-addr error. ret=%d", ret);
        return ret;
    }

    sock_nfd = st_netfd_open_socket(sock);
    if(sock_nfd == NULL){
        ret = ERROR_OPEN_SOCKET;
        Error("st_netfd_open_socket failed. ret=%d", ret);
        return ret;
    }
    Info("create socket(%d) success", sock);

    // connect to server
    sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(port);
    addr.sin_addr.s_addr = inet_addr(ip);

    if(st_connect(sock_nfd, (const struct sockaddr*)&addr, sizeof(sockaddr_in), ST_UTIME_NO_TIMEOUT) == -1){
        ret = ERROR_CONNECT;
        Error("connect to server(%s:%d) error. ret=%d", ip, port, ret);
        return ret;
    }
    Info("connec to server %s at port %d success", ip, port);

    status = SocketConnected;

    return ret;
}

int StSocket::Read(const void* buf, size_t size, ssize_t* nread){
    int ret = ERROR_SUCCESS;

    *nread = st_read(sock_nfd, (void*)buf, size, ST_UTIME_NO_TIMEOUT);

    if(*nread <= 0){
        if(*nread == 0){
            errno = ECONNRESET;
        }

        ret = ERROR_READ;
        status = SocketDisconnected;
    }

    if(*nread > 0){
        statistic->OnRead(context->GetId(), *nread);
    }

    return ret;
}

int StSocket::Write(const void* buf, size_t size, ssize_t* nwrite){
    int ret = ERROR_SUCCESS;

    *nwrite = st_write(sock_nfd, (void*)buf, size, ST_UTIME_NO_TIMEOUT);

    if(*nwrite <= 0){
        ret = ERROR_SEND;
        status = SocketDisconnected;
    }

    if(*nwrite > 0){
        statistic->OnWrite(context->GetId(), *nwrite);
    }

    return ret;
}

int StSocket::Close(){
    int ret = ERROR_SUCCESS;

    if(sock_nfd == NULL){
        return ret;
    }

    if(st_netfd_close(sock_nfd) != 0){
        ret = ERROR_CLOSE;
    }

    sock_nfd = NULL;
    status = SocketDisconnected;

    return ret;
}

int64_t StUtility::GetCurrentTime(){
    timeval now;

    int ret = gettimeofday(&now, NULL);

    if(ret == -1){
        Warn("gettimeofday error, ret=%d", ret);
    }

    // we must convert the tv_sec/tv_usec to int64_t.
    return ((int64_t)now.tv_sec)*1000 + ((int64_t)now.tv_usec) / 1000;
}

void StUtility::InitRandom(){
    timeval now;

    if(gettimeofday(&now, NULL) == -1){
        srand(0);
        return;
    }

    srand(now.tv_sec * 1000000 + now.tv_usec);
}

st_utime_t StUtility::BuildRandomMTime(double sleep_seconds){
    if(sleep_seconds <= 0){
        return 0 * 1000;
    }

    // 80% consts value.
    // 40% random value.
    // to get more graceful random time to mocking HLS client.
    st_utime_t sleep_ms = (int)(sleep_seconds * 1000 * 0.7) + rand() % (int)(sleep_seconds * 1000 * 0.4);

    return sleep_ms;
}

int StUtility::DnsResolve(string host, string& ip){
    int ret = ERROR_SUCCESS;

    if(inet_addr(host.c_str()) != INADDR_NONE){
        ip = host;
        Info("dns resolve %s to %s", host.c_str(), ip.c_str());

        return ret;
    }

    hostent* answer = gethostbyname(host.c_str());
    if(answer == NULL){
        ret = ERROR_DNS_RESOLVE;
        Error("dns resolve host %s error. ret=%d", host.c_str(), ret);
        return ret;
    }

    char ipv4[16];
    memset(ipv4, 0, sizeof(ipv4));
    for(int i = 0; i < answer->h_length; i++){
        inet_ntop(AF_INET, answer->h_addr_list[i], ipv4, sizeof(ipv4));
        Info("dns resolve host %s to %s.", host.c_str(), ipv4);
        break;
    }

    ip = ipv4;
    Info("dns resolve %s to %s", host.c_str(), ip.c_str());

    return ret;
}

StLogContext::StLogContext(){
}

StLogContext::~StLogContext(){
}

void StLogContext::SetId(int id){
    cache[st_thread_self()] = id;
}

int StLogContext::GetId(){
    return cache[st_thread_self()];
}
<|endoftext|>


<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
 */

#include <osg/Sequence>
#include <osg/Notify>

using namespace osg;

/**
 * Sequence constructor.
 */
Sequence::Sequence() :
    Group(),
    _value(-1),
    _now(0.0),
    _start(-1.0),
    _totalTime(0.),
    _resetTotalTime(true),
    _loopMode(LOOP),
    _begin(0),
    _end(-1),
    _speed(0),
    _nreps(-1),
    _nrepsRemain(-1),
    _step(0),
    _defaultTime(1.),
    _lastFrameTime(0.),
    _saveRealLastFrameTime(-1.),
    _mode(STOP),
    _sync(false),
    _clearOnStop(false)

{
    setNumChildrenRequiringUpdateTraversal(1);
}

Sequence::Sequence(const Sequence& seq, const CopyOp& copyop) :
    Group(seq, copyop),
    _value(seq._value),
    _now(seq._now),
    _start(seq._start),
    _frameTime(seq._frameTime),
    _totalTime(seq._totalTime),
    _resetTotalTime(seq._resetTotalTime),
    _loopMode(seq._loopMode),
    _begin(seq._begin),
    _end(seq._end),
    _speed(seq._speed),
    _nreps(seq._nreps),
    _nrepsRemain(seq._nrepsRemain),
    _step(seq._step),
    _defaultTime(seq._defaultTime),
    _lastFrameTime(seq._lastFrameTime),
    _saveRealLastFrameTime(seq._saveRealLastFrameTime),
    _saveRealLastFrameValue(seq._saveRealLastFrameValue),
    _mode(seq._mode),
    _sync(seq._sync),
    _clearOnStop(seq._clearOnStop)
{
    setNumChildrenRequiringUpdateTraversal(getNumChildrenRequiringUpdateTraversal()+1);
}

bool Sequence::addChild( Node *child)
{
    return Sequence::insertChild( _children.size(), child, _defaultTime);
}

bool Sequence::addChild( Node *child, double t)
{
    return Sequence::insertChild( _children.size(), child, t);
}

bool Sequence::insertChild( unsigned int index, Node *child)
{
    return Sequence::insertChild(index, child, _defaultTime);
}

bool Sequence::insertChild( unsigned int index, Node *child, double t)
{
    if (Group::insertChild(index,child))
    {
        if (index>=_frameTime.size())
        {
            Sequence::setTime(index, t);
        }
        _resetTotalTime = true;
        return true;
    }
    return false;
}

bool Sequence::removeChild( Node *child )
{
    if (Group::removeChild(child ))
    {
        unsigned int pos = getChildIndex(child);
        if (pos < _children.size())
            return Sequence::removeChildren(pos,1);
        else
            return false;
    }
    else
        return false;
}

bool Sequence::removeChildren(unsigned int pos,unsigned int numChildrenToRemove)
{
    if (pos<_frameTime.size())
        _frameTime.erase(_frameTime.begin()+pos,
                         osg::minimum(_frameTime.begin()+(pos+numChildrenToRemove),
                                      _frameTime.end()) );
    _resetTotalTime = true;
    return Group::removeChildren(pos,numChildrenToRemove);
}

// if frame >= _frameTime.size() then extend _frameTime to have frame-1 items
// a time <0 will get set to 0
void Sequence::setTime(unsigned int frame, double t)
{
    if (t<0.) t = 0.0;
    unsigned int sz = _frameTime.size();
    if (frame < sz)
    {
        _frameTime[frame] = t;
    }
    else
    {
        for (unsigned int i = sz; i <= frame; i++)
        {
            _frameTime.push_back(t);
        }
    }

}

// returns a frame time of -1 if frame is out of range
double Sequence::getTime(unsigned int frame) const
{
    if (frame < _frameTime.size())
        return _frameTime[frame];
    else
        return -1.0f;
}

void Sequence::setInterval(LoopMode mode, int begin, int end)
{
    _loopMode = mode;
    _end = end;
    _begin = begin;

    // _value based on _begin & _end
    _value = -1;

    _resetTotalTime = true;

}

void Sequence::setDuration(float speed, int nreps)
{
    _speed = speed;
    // -1 means loop forever
    _nreps = (nreps < 0 ? -1:nreps);
    // countdown of laps around the track
    _nrepsRemain = _nreps;
}

void Sequence::setMode(SequenceMode mode)
{
    int ubegin, uend;

    switch (mode)
    {
    case START:
        // restarts sequence from beginning
        _value = -1;

        // Figure out which direction to start stepping the sequence
        ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
        uend = (_end < 0 ? (int)_frameTime.size()-1: _end);
        _step = (ubegin > uend ? -1 : 1);

        _start = -1.0;
        _mode = mode;
        if (_saveRealLastFrameTime>=0.)
        {
            _frameTime[_saveRealLastFrameValue] = _saveRealLastFrameTime;
            _saveRealLastFrameTime = -1.;
        }
        break;
    case STOP:
        _mode = mode;
        break;
    case PAUSE:
        if (_mode == START)
            _mode = PAUSE;
        break;
    case RESUME:
        if (_mode == PAUSE)
            _mode = START;
        break;
    }
}

void Sequence::traverse(NodeVisitor& nv)
{
    if (getNumChildren()==0) return;

    const FrameStamp* framestamp = nv.getFrameStamp();
    if (framestamp)
    {
        _now = framestamp->getSimulationTime();
    }

    if (nv.getVisitorType()==NodeVisitor::UPDATE_VISITOR &&
        _mode == START &&
        !_frameTime.empty() && getNumChildren()!=0)
    {

        // if begin or end < 0, make it last frame
        int _ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
        int _uend = (_end < 0 ? (int)_frameTime.size()-1: _end);

        int _sbegin = osg::minimum(_ubegin,_uend);
        int _send = osg::maximum(_ubegin,_uend);

        if (framestamp)
        {
            // hack for last frame time
            if (_lastFrameTime>0. && _nrepsRemain==1 && _saveRealLastFrameTime<0.)
            {
                if ( _loopMode == LOOP)
                {
                    if ((_step>0 && _value!=_send) || (_step<0 && _value!=_sbegin))
                    {
                        _saveRealLastFrameTime=_frameTime[_uend];
                        _saveRealLastFrameValue = _uend;
                        _frameTime[_uend] = _lastFrameTime;
                        _resetTotalTime = true;
                    }
                }
                else
                {
                    if (_step>0 && _value!=_sbegin)
                    {
                        _saveRealLastFrameTime=_frameTime[_send];
                        _saveRealLastFrameValue = _send;
                        _frameTime[_send] = _lastFrameTime;
                        _resetTotalTime = true;
                    }
                    else if (_step<0 && _value!=_send)
                    {
                        _saveRealLastFrameTime=_frameTime[_sbegin];
                        _saveRealLastFrameValue = _sbegin;
                        _frameTime[_sbegin] = _lastFrameTime;
                        _resetTotalTime = true;
                    }
                }
            }

            // I never know when to stop!
            // more fun for last frame time
            if (_nrepsRemain==0)
            {
                if (!_clearOnStop)
                {
                    _mode = STOP;
                }
                else
                {
                    if ( (_loopMode == LOOP) &&
                         ( (_step>0 && _value!=_send) ||
                           (_step<0 && _value!=_sbegin)))
                    {
                        _mode = STOP;
                    }
                    else if ( (_loopMode == SWING) &&
                              ( (_step<0 && _value!=_send) ||
                                (_step>0 && _value!=_sbegin)))
                    {
                        _mode = STOP;
                    }

                }
            }

            // update local variables
            _update();

            // now for the heavy lifting! three options
            // 1) still in the same frame, so have nothing to do
            // 2) just in the next frame
            // 3) need to calculate everything based on elapsed time
            if ((_now - _start) > _frameTime[_value]*osg::absolute(_speed))
            { // case 2 or case 3
                // most of the time it's just the next frame in the sequence
                int nextValue = _getNextValue();
                if (!_sync ||
                    ((_now - _start) <= (_frameTime[_value]+_frameTime[nextValue])*osg::absolute(_speed)) )
                {
                    _start += _frameTime[_value]*osg::absolute(_speed);
                    // repeat or change directions?
                    if ( (_step>0 && nextValue==_send) ||
                         (_step<0 && nextValue==_sbegin))
                    {
                        if (_nreps>0)
                            _nrepsRemain--;

                        // change direction
                        if  (_loopMode == SWING)
                            _step = -_step;

                    }
                    _value = nextValue;
                }
                else // case 3
        {
            // recalculate everything based on elapsed time

            // elapsed time from start of the frame
            double deltaT = _now - _start;

            // factors _speed into account
            double adjTotalTime = _totalTime*osg::absolute(_speed);

            // how many laps?
            int loops = (int)(deltaT/adjTotalTime);

            // adjust reps & quick check to see if done because reps used up

            if (_nreps>0)
            {
            if (_loopMode == LOOP)
                _nrepsRemain -= loops;
            else
                _nrepsRemain -= 2*loops;

            if (_nrepsRemain<=0)
            {
                _nrepsRemain = 0;
                _mode = STOP;
                OSG_WARN << "stopping because elapsed time greater or equal to time remaining to repeat the sequence\n";
            }
            }

            // deduct off time for laps- _value shouldn't change as it's modulo the total time
            double jumpStart = ((double)loops * adjTotalTime);

            // step through frames one at a time until caught up
            while (deltaT-jumpStart > _frameTime[_value]*osg::absolute(_speed))
            {
            jumpStart +=  _frameTime[_value]*osg::absolute(_speed );
            _value = _getNextValue();
            }

            // set start time
            _start += jumpStart;
        }
            }
        }
        else
            OSG_WARN << "osg::Sequence::traverse(NodeVisitor&) requires a valid FrameStamp to function, sequence not updated.\n";

    }

    // now do the traversal
    if (nv.getTraversalMode()==NodeVisitor::TRAVERSE_ACTIVE_CHILDREN)
    {
        if ( !((_mode == STOP) && _clearOnStop) &&
             (getValue()>=0 && getValue()<(int)_children.size()) )
        {
            _children[getValue()]->accept(nv);
        }
    }
    else
    {
        Group::traverse(nv);
    }

}

int Sequence::_getNextValue()
{
    if (_frameTime.empty() || getNumChildren()==0) return 0;

    // if begin or end < 0, make it last frame
    int _ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
    int _uend = (_end < 0 ? (int)_frameTime.size()-1: _end);

    int _sbegin = osg::minimum(_ubegin,_uend);
    int _send = osg::maximum(_ubegin,_uend);

    int v = _value + _step * static_cast<int>(osg::sign(_speed));

    if (_sbegin==_send)
    {
        return _sbegin;
    }
    else if (v<=_send && v>=_sbegin)
    {
        return v;
    }
    else
    {
        int vs = _send - _sbegin + 1;
        if (_loopMode == LOOP)
        {
            v = ((v-_sbegin)%vs) + _sbegin;
            if (v<_sbegin)
            {
                v+=vs;
            }

            return v;
        }
        else // SWING
        {
            if (v>_send)
                return (2*_send-v);
            else
                return (2*_sbegin-v);

        }
    }

}

void Sequence::_update()
{
    if (_frameTime.empty()) return;

    // if begin or end < 0, make it last frame
    int _ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
    int _uend = (_end < 0 ? (int)_frameTime.size()-1: _end);

    int _sbegin = osg::minimum(_ubegin,_uend);
    int _send = osg::maximum(_ubegin,_uend);

    // if _value<0, new or restarted
    if (_value<0)
    {
        _value = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
        _resetTotalTime = true;
    }

    // if _start<0, new or restarted
    if (_start<0)
    {
        _start = _now;
        _resetTotalTime = true;
    }

    // need to calculate time of a complete sequence?
    // time is different depending on loop mode
    if (_resetTotalTime)
    {
        if (_loopMode == LOOP)
        {
            _totalTime = 0.0;
            for (int i=_sbegin; i<=_send; i++)
            {
                _totalTime += _frameTime[i];
            }
        }
        else //SWING
        {
            _totalTime = _frameTime[_sbegin];
            // ones in the middle get counted twice: 0 1 2 3 4 3 2 1 0
            for (int i=_sbegin+1; i<_send; i++)
            {
                _totalTime += 2*_frameTime[i];
            }
            if (_sbegin != _send)
            {
                _totalTime += _frameTime[_send];
            }
        }

        _resetTotalTime = false;
    }

}
<commit_msg>Added initializer<commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
 */

#include <osg/Sequence>
#include <osg/Notify>

using namespace osg;

/**
 * Sequence constructor.
 */
Sequence::Sequence() :
    Group(),
    _value(-1),
    _now(0.0),
    _start(-1.0),
    _totalTime(0.),
    _resetTotalTime(true),
    _loopMode(LOOP),
    _begin(0),
    _end(-1),
    _speed(0),
    _nreps(-1),
    _nrepsRemain(-1),
    _step(0),
    _defaultTime(1.),
    _lastFrameTime(0.),
    _saveRealLastFrameTime(-1.),
    _saveRealLastFrameValue(0),
    _mode(STOP),
    _sync(false),
    _clearOnStop(false)

{
    setNumChildrenRequiringUpdateTraversal(1);
}

Sequence::Sequence(const Sequence& seq, const CopyOp& copyop) :
    Group(seq, copyop),
    _value(seq._value),
    _now(seq._now),
    _start(seq._start),
    _frameTime(seq._frameTime),
    _totalTime(seq._totalTime),
    _resetTotalTime(seq._resetTotalTime),
    _loopMode(seq._loopMode),
    _begin(seq._begin),
    _end(seq._end),
    _speed(seq._speed),
    _nreps(seq._nreps),
    _nrepsRemain(seq._nrepsRemain),
    _step(seq._step),
    _defaultTime(seq._defaultTime),
    _lastFrameTime(seq._lastFrameTime),
    _saveRealLastFrameTime(seq._saveRealLastFrameTime),
    _saveRealLastFrameValue(seq._saveRealLastFrameValue),
    _mode(seq._mode),
    _sync(seq._sync),
    _clearOnStop(seq._clearOnStop)
{
    setNumChildrenRequiringUpdateTraversal(getNumChildrenRequiringUpdateTraversal()+1);
}

bool Sequence::addChild( Node *child)
{
    return Sequence::insertChild( _children.size(), child, _defaultTime);
}

bool Sequence::addChild( Node *child, double t)
{
    return Sequence::insertChild( _children.size(), child, t);
}

bool Sequence::insertChild( unsigned int index, Node *child)
{
    return Sequence::insertChild(index, child, _defaultTime);
}

bool Sequence::insertChild( unsigned int index, Node *child, double t)
{
    if (Group::insertChild(index,child))
    {
        if (index>=_frameTime.size())
        {
            Sequence::setTime(index, t);
        }
        _resetTotalTime = true;
        return true;
    }
    return false;
}

bool Sequence::removeChild( Node *child )
{
    if (Group::removeChild(child ))
    {
        unsigned int pos = getChildIndex(child);
        if (pos < _children.size())
            return Sequence::removeChildren(pos,1);
        else
            return false;
    }
    else
        return false;
}

bool Sequence::removeChildren(unsigned int pos,unsigned int numChildrenToRemove)
{
    if (pos<_frameTime.size())
        _frameTime.erase(_frameTime.begin()+pos,
                         osg::minimum(_frameTime.begin()+(pos+numChildrenToRemove),
                                      _frameTime.end()) );
    _resetTotalTime = true;
    return Group::removeChildren(pos,numChildrenToRemove);
}

// if frame >= _frameTime.size() then extend _frameTime to have frame-1 items
// a time <0 will get set to 0
void Sequence::setTime(unsigned int frame, double t)
{
    if (t<0.) t = 0.0;
    unsigned int sz = _frameTime.size();
    if (frame < sz)
    {
        _frameTime[frame] = t;
    }
    else
    {
        for (unsigned int i = sz; i <= frame; i++)
        {
            _frameTime.push_back(t);
        }
    }

}

// returns a frame time of -1 if frame is out of range
double Sequence::getTime(unsigned int frame) const
{
    if (frame < _frameTime.size())
        return _frameTime[frame];
    else
        return -1.0f;
}

void Sequence::setInterval(LoopMode mode, int begin, int end)
{
    _loopMode = mode;
    _end = end;
    _begin = begin;

    // _value based on _begin & _end
    _value = -1;

    _resetTotalTime = true;

}

void Sequence::setDuration(float speed, int nreps)
{
    _speed = speed;
    // -1 means loop forever
    _nreps = (nreps < 0 ? -1:nreps);
    // countdown of laps around the track
    _nrepsRemain = _nreps;
}

void Sequence::setMode(SequenceMode mode)
{
    int ubegin, uend;

    switch (mode)
    {
    case START:
        // restarts sequence from beginning
        _value = -1;

        // Figure out which direction to start stepping the sequence
        ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
        uend = (_end < 0 ? (int)_frameTime.size()-1: _end);
        _step = (ubegin > uend ? -1 : 1);

        _start = -1.0;
        _mode = mode;
        if (_saveRealLastFrameTime>=0.)
        {
            _frameTime[_saveRealLastFrameValue] = _saveRealLastFrameTime;
            _saveRealLastFrameTime = -1.;
        }
        break;
    case STOP:
        _mode = mode;
        break;
    case PAUSE:
        if (_mode == START)
            _mode = PAUSE;
        break;
    case RESUME:
        if (_mode == PAUSE)
            _mode = START;
        break;
    }
}

void Sequence::traverse(NodeVisitor& nv)
{
    if (getNumChildren()==0) return;

    const FrameStamp* framestamp = nv.getFrameStamp();
    if (framestamp)
    {
        _now = framestamp->getSimulationTime();
    }

    if (nv.getVisitorType()==NodeVisitor::UPDATE_VISITOR &&
        _mode == START &&
        !_frameTime.empty() && getNumChildren()!=0)
    {

        // if begin or end < 0, make it last frame
        int _ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
        int _uend = (_end < 0 ? (int)_frameTime.size()-1: _end);

        int _sbegin = osg::minimum(_ubegin,_uend);
        int _send = osg::maximum(_ubegin,_uend);

        if (framestamp)
        {
            // hack for last frame time
            if (_lastFrameTime>0. && _nrepsRemain==1 && _saveRealLastFrameTime<0.)
            {
                if ( _loopMode == LOOP)
                {
                    if ((_step>0 && _value!=_send) || (_step<0 && _value!=_sbegin))
                    {
                        _saveRealLastFrameTime=_frameTime[_uend];
                        _saveRealLastFrameValue = _uend;
                        _frameTime[_uend] = _lastFrameTime;
                        _resetTotalTime = true;
                    }
                }
                else
                {
                    if (_step>0 && _value!=_sbegin)
                    {
                        _saveRealLastFrameTime=_frameTime[_send];
                        _saveRealLastFrameValue = _send;
                        _frameTime[_send] = _lastFrameTime;
                        _resetTotalTime = true;
                    }
                    else if (_step<0 && _value!=_send)
                    {
                        _saveRealLastFrameTime=_frameTime[_sbegin];
                        _saveRealLastFrameValue = _sbegin;
                        _frameTime[_sbegin] = _lastFrameTime;
                        _resetTotalTime = true;
                    }
                }
            }

            // I never know when to stop!
            // more fun for last frame time
            if (_nrepsRemain==0)
            {
                if (!_clearOnStop)
                {
                    _mode = STOP;
                }
                else
                {
                    if ( (_loopMode == LOOP) &&
                         ( (_step>0 && _value!=_send) ||
                           (_step<0 && _value!=_sbegin)))
                    {
                        _mode = STOP;
                    }
                    else if ( (_loopMode == SWING) &&
                              ( (_step<0 && _value!=_send) ||
                                (_step>0 && _value!=_sbegin)))
                    {
                        _mode = STOP;
                    }

                }
            }

            // update local variables
            _update();

            // now for the heavy lifting! three options
            // 1) still in the same frame, so have nothing to do
            // 2) just in the next frame
            // 3) need to calculate everything based on elapsed time
            if ((_now - _start) > _frameTime[_value]*osg::absolute(_speed))
            { // case 2 or case 3
                // most of the time it's just the next frame in the sequence
                int nextValue = _getNextValue();
                if (!_sync ||
                    ((_now - _start) <= (_frameTime[_value]+_frameTime[nextValue])*osg::absolute(_speed)) )
                {
                    _start += _frameTime[_value]*osg::absolute(_speed);
                    // repeat or change directions?
                    if ( (_step>0 && nextValue==_send) ||
                         (_step<0 && nextValue==_sbegin))
                    {
                        if (_nreps>0)
                            _nrepsRemain--;

                        // change direction
                        if  (_loopMode == SWING)
                            _step = -_step;

                    }
                    _value = nextValue;
                }
                else // case 3
        {
            // recalculate everything based on elapsed time

            // elapsed time from start of the frame
            double deltaT = _now - _start;

            // factors _speed into account
            double adjTotalTime = _totalTime*osg::absolute(_speed);

            // how many laps?
            int loops = (int)(deltaT/adjTotalTime);

            // adjust reps & quick check to see if done because reps used up

            if (_nreps>0)
            {
            if (_loopMode == LOOP)
                _nrepsRemain -= loops;
            else
                _nrepsRemain -= 2*loops;

            if (_nrepsRemain<=0)
            {
                _nrepsRemain = 0;
                _mode = STOP;
                OSG_WARN << "stopping because elapsed time greater or equal to time remaining to repeat the sequence\n";
            }
            }

            // deduct off time for laps- _value shouldn't change as it's modulo the total time
            double jumpStart = ((double)loops * adjTotalTime);

            // step through frames one at a time until caught up
            while (deltaT-jumpStart > _frameTime[_value]*osg::absolute(_speed))
            {
            jumpStart +=  _frameTime[_value]*osg::absolute(_speed );
            _value = _getNextValue();
            }

            // set start time
            _start += jumpStart;
        }
            }
        }
        else
            OSG_WARN << "osg::Sequence::traverse(NodeVisitor&) requires a valid FrameStamp to function, sequence not updated.\n";

    }

    // now do the traversal
    if (nv.getTraversalMode()==NodeVisitor::TRAVERSE_ACTIVE_CHILDREN)
    {
        if ( !((_mode == STOP) && _clearOnStop) &&
             (getValue()>=0 && getValue()<(int)_children.size()) )
        {
            _children[getValue()]->accept(nv);
        }
    }
    else
    {
        Group::traverse(nv);
    }

}

int Sequence::_getNextValue()
{
    if (_frameTime.empty() || getNumChildren()==0) return 0;

    // if begin or end < 0, make it last frame
    int _ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
    int _uend = (_end < 0 ? (int)_frameTime.size()-1: _end);

    int _sbegin = osg::minimum(_ubegin,_uend);
    int _send = osg::maximum(_ubegin,_uend);

    int v = _value + _step * static_cast<int>(osg::sign(_speed));

    if (_sbegin==_send)
    {
        return _sbegin;
    }
    else if (v<=_send && v>=_sbegin)
    {
        return v;
    }
    else
    {
        int vs = _send - _sbegin + 1;
        if (_loopMode == LOOP)
        {
            v = ((v-_sbegin)%vs) + _sbegin;
            if (v<_sbegin)
            {
                v+=vs;
            }

            return v;
        }
        else // SWING
        {
            if (v>_send)
                return (2*_send-v);
            else
                return (2*_sbegin-v);

        }
    }

}

void Sequence::_update()
{
    if (_frameTime.empty()) return;

    // if begin or end < 0, make it last frame
    int _ubegin = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
    int _uend = (_end < 0 ? (int)_frameTime.size()-1: _end);

    int _sbegin = osg::minimum(_ubegin,_uend);
    int _send = osg::maximum(_ubegin,_uend);

    // if _value<0, new or restarted
    if (_value<0)
    {
        _value = (_begin < 0 ?  (int)_frameTime.size()-1: _begin);
        _resetTotalTime = true;
    }

    // if _start<0, new or restarted
    if (_start<0)
    {
        _start = _now;
        _resetTotalTime = true;
    }

    // need to calculate time of a complete sequence?
    // time is different depending on loop mode
    if (_resetTotalTime)
    {
        if (_loopMode == LOOP)
        {
            _totalTime = 0.0;
            for (int i=_sbegin; i<=_send; i++)
            {
                _totalTime += _frameTime[i];
            }
        }
        else //SWING
        {
            _totalTime = _frameTime[_sbegin];
            // ones in the middle get counted twice: 0 1 2 3 4 3 2 1 0
            for (int i=_sbegin+1; i<_send; i++)
            {
                _totalTime += 2*_frameTime[i];
            }
            if (_sbegin != _send)
            {
                _totalTime += _frameTime[_send];
            }
        }

        _resetTotalTime = false;
    }

}
<|endoftext|>


<commit_before>#include <Eigen/Dense>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <SimDynamics/SimDynamics.h>

#include <boost/assert.hpp>
#include <boost/make_shared.hpp>

#include "kajita/ChestPostureController.h"
#include "kajita/FootForceController.h"
#include "kajita/FootTorqueController.h"

#include "stabilizer/kajita/ForceDistributor.h"
#include "stabilizer/KajitaStabilizer.h"

#include "ik/DifferentialReferenceIK.h"

/*
 * These controller needs a trajectory with the following control features:
 *
 * Poses:
 *  - chest pose
 *  - pelvis pose
 *  - left foot pose
 *  - right foot pose
 *
 * Values:
 *  - support phase
 *
 * Points:
 *  - Reference ZMP
 */
KajitaStabilizer::KajitaStabilizer(const VirtualRobot::RobotPtr& robot,
               const VirtualRobot::ForceTorqueSensorPtr& leftAnkleSensor,
               const VirtualRobot::ForceTorqueSensorPtr& rightAnkleSensor)
// Names specific to ARMAR 4
: chest(robot->getRobotNode("TorsoCenter"))
, leftFoot(robot->getRobotNode("LeftLeg_TCP"))
, rightFoot(robot->getRobotNode("RightLeg_TCP"))
, leftAnkle(robot->getRobotNode("LeftLeg_BodyAnkle2"))
, rightAnkle(robot->getRobotNode("RightLeg_BodyAnkle2"))
, pelvis(robot->getRobotNode("Waist"))
, leftAnkleSensor(leftAnkleSensor)
, rightAnkleSensor(rightAnkleSensor)
, chestPostureController(new ChestPostureController())
, forceDistributor(new ForceDistributor(robot->getMass(),
                                        Eigen::Vector3f(0.0, 0.0, -9.81),
                                        leftAnkle, rightAnkle, leftFoot, rightFoot))
, footTorqueController(new FootTorqueController())
, nodes(robot->getRobotNodeSet("CoMCompensation"))
, referenceIK(boost::dynamic_pointer_cast<ReferenceIK>(
                boost::make_shared<DifferentialReferenceIK>(nodes, robot, robot->getRobotNodeSet("ColModelAll"), leftFoot, rightFoot, chest, pelvis)
              ))
, chestPose(Eigen::Matrix4f::Identity())
, pelvisPose(Eigen::Matrix4f::Identity())
, leftFootPose(Eigen::Matrix4f::Identity())
, rightFootPose(Eigen::Matrix4f::Identity())
, rootPose(Eigen::Matrix4f::Identity())
, comPosition(Eigen::Vector3f::Zero())
, comPositionRef(Eigen::Vector3f::Zero())
, ft({Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()})
{
    Eigen::Vector3f leftHipPos  = robot->getRobotNode("LeftLeg_Joint2")->getGlobalPose().block(0, 3, 3, 1);
    Eigen::Vector3f rightHipPos = robot->getRobotNode("RightLeg_Joint2")->getGlobalPose().block(0, 3, 3, 1);
    double hipJointDistance = (leftHipPos - rightHipPos).norm();
    footForceController = FootForceControllerPtr(new FootForceController(hipJointDistance));

}

void KajitaStabilizer::update(float dt,
                              Kinematics::SupportPhase phase,
                              const Eigen::Vector3f& zmpRefWorld,
                              const Eigen::Matrix4f& chestPoseRefWorld,
                              const Eigen::Matrix4f& pelvisPoseRefWorld,
                              const Eigen::Matrix4f& leftFootPoseRefWorld,
                              const Eigen::Matrix4f& rightFootPoseRefWorld,
                              const Eigen::Vector3f& comRefWorld)
{
    Eigen::Matrix4f stepAdaptionFrame =
        computeGroundFrame(leftFoot->getGlobalPose(), rightFoot->getGlobalPose(), phase)
      * computeGroundFrame(leftFootPoseRefWorld, rightFootPoseRefWorld, phase).inverse();
    //Eigen::Matrix4f stepAdaptionFrame = Eigen::Matrix4f::Identity();

    chestPoseRef     = stepAdaptionFrame * chestPoseRefWorld;
    pelvisPoseRef    = stepAdaptionFrame * pelvisPoseRefWorld;
    leftFootPoseRef  = stepAdaptionFrame * leftFootPoseRefWorld;
    rightFootPoseRef = stepAdaptionFrame * rightFootPoseRefWorld;
    zmpPositionRef   = VirtualRobot::MathTools::transformPosition(zmpRefWorld, stepAdaptionFrame);
    comPositionRef   = VirtualRobot::MathTools::transformPosition(comRefWorld, stepAdaptionFrame);
    //chestPoseRef     = chestPoseRefWorld;
    //pelvisPoseRef    = pelvisPoseRefWorld;
    //leftFootPoseRef  = leftFootPoseRefWorld;
    //rightFootPoseRef = rightFootPoseRefWorld;

    ft = forceDistributor->distributeZMP(
        leftFoot->getGlobalPose(),
        rightFoot->getGlobalPose(),
        zmpPositionRef,
        phase
    );

    leftFootPose  = Eigen::Matrix4f::Identity();
    rightFootPose = Eigen::Matrix4f::Identity();
    footTorqueController->correctFootOrientation(
        leftFootPoseRef,
        rightFootPoseRef,
        ft.leftTorque,
        ft.rightTorque,
        leftAnkleSensor->getTorque(),
        rightAnkleSensor->getTorque(),
        leftFootPose,
        rightFootPose
    );

    pelvisPose = footForceController->correctPelvisOrientation(
        pelvisPoseRef,
        ft.leftForce,
        ft.rightForce,
        leftAnkleSensor->getForce(),
        rightAnkleSensor->getForce()
    );

    chestPose = chestPostureController->correctPosture(
        chestPoseRef,
        chest->getGlobalPose()
    );

    leftFootPose  = leftFootPoseRef;
    rightFootPose = rightFootPoseRef;
    chestPose     = chestPoseRef;
    pelvisPose    = pelvisPoseRef;
    comPosition   = comPositionRef;

    bool success = referenceIK->computeStep(leftFootPose, rightFootPose, chestPose, pelvisPose, comPosition, phase, resultAngles);
    BOOST_ASSERT(!std::isnan(resultAngles[0]));
}

VirtualRobot::RobotPtr KajitaStabilizer::getInvertedRobot()
{
    return boost::dynamic_pointer_cast<DifferentialReferenceIK>(referenceIK)->getInvertedRobot();
}
<commit_msg>Use chest posture correction<commit_after>#include <Eigen/Dense>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
#include <SimDynamics/SimDynamics.h>

#include <boost/assert.hpp>
#include <boost/make_shared.hpp>

#include "kajita/ChestPostureController.h"
#include "kajita/FootForceController.h"
#include "kajita/FootTorqueController.h"

#include "stabilizer/kajita/ForceDistributor.h"
#include "stabilizer/KajitaStabilizer.h"

#include "ik/DifferentialReferenceIK.h"

/*
 * These controller needs a trajectory with the following control features:
 *
 * Poses:
 *  - chest pose
 *  - pelvis pose
 *  - left foot pose
 *  - right foot pose
 *
 * Values:
 *  - support phase
 *
 * Points:
 *  - Reference ZMP
 */
KajitaStabilizer::KajitaStabilizer(const VirtualRobot::RobotPtr& robot,
               const VirtualRobot::ForceTorqueSensorPtr& leftAnkleSensor,
               const VirtualRobot::ForceTorqueSensorPtr& rightAnkleSensor)
// Names specific to ARMAR 4
: chest(robot->getRobotNode("TorsoCenter"))
, leftFoot(robot->getRobotNode("LeftLeg_TCP"))
, rightFoot(robot->getRobotNode("RightLeg_TCP"))
, leftAnkle(robot->getRobotNode("LeftLeg_BodyAnkle2"))
, rightAnkle(robot->getRobotNode("RightLeg_BodyAnkle2"))
, pelvis(robot->getRobotNode("Waist"))
, leftAnkleSensor(leftAnkleSensor)
, rightAnkleSensor(rightAnkleSensor)
, chestPostureController(new ChestPostureController())
, forceDistributor(new ForceDistributor(robot->getMass(),
                                        Eigen::Vector3f(0.0, 0.0, -9.81),
                                        leftAnkle, rightAnkle, leftFoot, rightFoot))
, footTorqueController(new FootTorqueController())
, nodes(robot->getRobotNodeSet("CoMCompensation"))
, referenceIK(boost::dynamic_pointer_cast<ReferenceIK>(
                boost::make_shared<DifferentialReferenceIK>(nodes, robot, robot->getRobotNodeSet("ColModelAll"), leftFoot, rightFoot, chest, pelvis)
              ))
, chestPose(Eigen::Matrix4f::Identity())
, pelvisPose(Eigen::Matrix4f::Identity())
, leftFootPose(Eigen::Matrix4f::Identity())
, rightFootPose(Eigen::Matrix4f::Identity())
, rootPose(Eigen::Matrix4f::Identity())
, comPosition(Eigen::Vector3f::Zero())
, comPositionRef(Eigen::Vector3f::Zero())
, ft({Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()})
{
    Eigen::Vector3f leftHipPos  = robot->getRobotNode("LeftLeg_Joint2")->getGlobalPose().block(0, 3, 3, 1);
    Eigen::Vector3f rightHipPos = robot->getRobotNode("RightLeg_Joint2")->getGlobalPose().block(0, 3, 3, 1);
    double hipJointDistance = (leftHipPos - rightHipPos).norm();
    footForceController = FootForceControllerPtr(new FootForceController(hipJointDistance));

}

void KajitaStabilizer::update(float dt,
                              Kinematics::SupportPhase phase,
                              const Eigen::Vector3f& zmpRefWorld,
                              const Eigen::Matrix4f& chestPoseRefWorld,
                              const Eigen::Matrix4f& pelvisPoseRefWorld,
                              const Eigen::Matrix4f& leftFootPoseRefWorld,
                              const Eigen::Matrix4f& rightFootPoseRefWorld,
                              const Eigen::Vector3f& comRefWorld)
{
    Eigen::Matrix4f stepAdaptionFrame =
        computeGroundFrame(leftFoot->getGlobalPose(), rightFoot->getGlobalPose(), phase)
      * computeGroundFrame(leftFootPoseRefWorld, rightFootPoseRefWorld, phase).inverse();

    chestPoseRef     = stepAdaptionFrame * chestPoseRefWorld;
    pelvisPoseRef    = stepAdaptionFrame * pelvisPoseRefWorld;
    leftFootPoseRef  = stepAdaptionFrame * leftFootPoseRefWorld;
    rightFootPoseRef = stepAdaptionFrame * rightFootPoseRefWorld;
    zmpPositionRef   = VirtualRobot::MathTools::transformPosition(zmpRefWorld, stepAdaptionFrame);
    comPositionRef   = VirtualRobot::MathTools::transformPosition(comRefWorld, stepAdaptionFrame);
    //chestPoseRef     = chestPoseRefWorld;
    //pelvisPoseRef    = pelvisPoseRefWorld;
    //leftFootPoseRef  = leftFootPoseRefWorld;
    //rightFootPoseRef = rightFootPoseRefWorld;

    ft = forceDistributor->distributeZMP(
        leftFoot->getGlobalPose(),
        rightFoot->getGlobalPose(),
        zmpPositionRef,
        phase
    );

    leftFootPose  = Eigen::Matrix4f::Identity();
    rightFootPose = Eigen::Matrix4f::Identity();
    footTorqueController->correctFootOrientation(
        leftFootPoseRef,
        rightFootPoseRef,
        ft.leftTorque,
        ft.rightTorque,
        leftAnkleSensor->getTorque(),
        rightAnkleSensor->getTorque(),
        leftFootPose,
        rightFootPose
    );

    pelvisPose = footForceController->correctPelvisOrientation(
        pelvisPoseRef,
        ft.leftForce,
        ft.rightForce,
        leftAnkleSensor->getForce(),
        rightAnkleSensor->getForce()
    );

    chestPose = chestPostureController->correctPosture(
        chestPoseRef,
        chest->getGlobalPose()
    );

    leftFootPose  = leftFootPoseRef;
    rightFootPose = rightFootPoseRef;
/*
    chestPose     = chestPoseRef;
    pelvisPose    = pelvisPoseRef;
*/
    comPosition   = comPositionRef;

    bool success = referenceIK->computeStep(leftFootPose, rightFootPose, chestPose, pelvisPose, comPosition, phase, resultAngles);
    BOOST_ASSERT(!std::isnan(resultAngles[0]));
}

VirtualRobot::RobotPtr KajitaStabilizer::getInvertedRobot()
{
    return boost::dynamic_pointer_cast<DifferentialReferenceIK>(referenceIK)->getInvertedRobot();
}
<|endoftext|>