C++ Movement::mid方法代码示例

/*
 * s_j + r CT + 2*|R| CT (3-c_{i,j}^{k1,k2,r}-m_i^k1-m_j^k2) \geq s_i + d_i (16)
 * s_i + 2*|R| CT (2+c_{i,j}^{k1,k2,r}-m_i^k1-m_j^k2) \geq s_j + d_j + r CT (17)
 */
void ConstraintsGenerator::addCollisions(const map1to1& s, const map1to1& d, const map2to1& x, const map2to1& y, const map4toN& c)	{
	assert(mRobots.size() > 1 && mLine != nullptr && "More than one robot expected!");
	for (const pair<ActivityMode*, ActivityMode*>& col : mLine->collisions())	{
		int32_t numberOfRobots = mRobots.size();
		for (int32_t r = -numberOfRobots; r <= numberOfRobots; ++r)	{
			Operator op = GREATER_EQUAL;
			SparseMatrix<double>::Row row;
			double constVal = 2.0*numberOfRobots*mCycleTime;
			double b1 = -3.0*constVal-r*mCycleTime, b2 = -2.0*constVal+r*mCycleTime;

			uint32_t a[2], m[2], i = 0;
			for (ActivityMode* mode : {col.first, col.second})	{
				Movement *mv = dynamic_cast<Movement*>(mode);
				if (mv != nullptr)	{
					DynamicActivity *da = mv->parent();
					a[i] = da->aid(); m[i] = mv->mid();
					row.emplace_back(getValue(y, {a[i], m[i]}, caller()), -constVal);
				}

				Location *loc = dynamic_cast<Location*>(mode);
				if (loc != nullptr)	{
					StaticActivity *sa = loc->parent();
					a[i] = sa->aid(); m[i] = loc->lid();
					row.emplace_back(getValue(x, {a[i], loc->point()}, caller()), -constVal);
				}

				++i;
			}

			SparseMatrix<double>::Row row1 = row, row2 = row;
			const vector<uint32_t>& v = getValue(c, {pack(a[0], m[0]), pack(a[1], m[1])}, caller());
			row1.insert(row1.end(), {{getValue(s, a[1], caller()), 1.0}, {getValue(s, a[0], caller()), -1.0}, {getValue(d, a[0], caller()), -1.0}, {v[r+numberOfRobots], -constVal}});
			row2.insert(row2.end(), {{getValue(s, a[0], caller()), 1.0}, {getValue(s, a[1], caller()), -1.0}, {getValue(d, a[1], caller()), -1.0}, {v[r+numberOfRobots], constVal}});

			addConstraint(row1, op, b1, "(16$"+to_string(mConstraintsCounter++)+")");
			addConstraint(row2, op, b2, "(17$"+to_string(mConstraintsCounter++)+")");
		}
	}
}

DynamicActivity* XmlReader::processDynamicActivityNode(const Node *node) {

    const Element *activityElement = castToElement(node);
    DynamicActivity *dynamicActivity = new DynamicActivity(stoul(getAttribute(activityElement, "aid")));

    dynamicActivity->name(getTextFromElement(activityElement, "name", false));
    dynamicActivity->description(getTextFromElement(activityElement, "desc", false));

    double minDuration = F64_MAX, maxDuration = F64_MIN;
    const Element *movements = castToElement(activityElement->get_first_child("movements"));
    string a1 = getAttribute(movements, "from_aid", false), a2 = getAttribute(movements, "to_aid", false);
    int64_t fromActivity = (a1.empty() ? -1 : stol(a1)), toActivity = (a2.empty() ? -1 : stol(a2));

    const NodeSet movementSet = movements->find("movement");
    for (const Node *movementNode : movementSet)	{
        const Element *movementElement = castToElement(movementNode);

        Movement *mv = new Movement(stoul(getAttribute(movementElement, "mid")));
        mv->minDuration(stod(getTextFromElement(movementElement, "min-duration")));
        mv->maxDuration(stod(getTextFromElement(movementElement, "max-duration")));
        if (mv->minDuration() < TIME_ERR)	{
            string mid = to_string(mv->mid());
            delete dynamicActivity;
            delete mv;
            throw InvalidDatasetFile(caller(), "Dynamic activity's movement "+mid+" must have positive duration!", movementNode->get_line());
        }

        // Workaround the incapability of ILP solvers to solve the problem with the fixed variables.
        if (abs(mv->maxDuration()-mv->minDuration()) < TIME_ERR)
            mv->maxDuration(mv->minDuration()+TIME_ERR);

        const NodeSet monomials = movementElement->find("energy-function/monomial");
        for (const Node *monomialNode : monomials)	{
            const Element *monomialElement = castToElement(monomialNode);
            int32_t degree = stoi(getAttribute(monomialElement, "degree"));
            double coeff = stod(getAttribute(monomialElement, "coeff"));
            mv->addMonomial({degree, coeff});
        }

        uint32_t fromPoint = stoul(getTextFromElement(movementElement, "from-point"));
        uint32_t toPoint = stoul(getTextFromElement(movementElement, "to-point"));

        Location *locFrom = nullptr, *locTo = nullptr;
        StaticActivity *actFrom = nullptr, *actTo = nullptr;
        auto fSit = mPointToLocation.find(fromPoint), tSit = mPointToLocation.find(toPoint);
        if (fSit == mPointToLocation.cend() || tSit == mPointToLocation.cend())	{
            delete dynamicActivity;
            delete mv;
            throw InvalidDatasetFile(caller(), "Dynamic activity's point is not linked with any static activity!", movementNode->get_line());
        } else {
            locFrom = fSit->second;
            locTo = tSit->second;
            assert(locFrom != nullptr && locTo != nullptr && "Unexpected null pointers!");
            actFrom = locFrom->parent();
            actTo = locTo->parent();
            assert(actFrom != nullptr && actTo != nullptr && "Should not be nullptr as it has been set in processRobotNode method!");
        }

        if ((actFrom->aid() != fromActivity && fromActivity != -1) || (actTo->aid() != toActivity && toActivity != -1))	{
            delete dynamicActivity;
            delete mv;
            throw InvalidDatasetFile(caller(), "Optional attributes 'from_aid' and/or 'to_aid' are invalid!", movementNode->get_line());
        }

        minDuration = min(minDuration, mv->minDuration());
        maxDuration = max(maxDuration, mv->maxDuration());
        setValue(mMovementToPoints, mv, {fromPoint, toPoint}, caller());

        dynamicActivity->addMovement(mv);
    }

    dynamicActivity->minAbsDuration(minDuration);
    dynamicActivity->maxAbsDuration(maxDuration);

    return dynamicActivity;
}