const Pose2d operator-(const Pose2d& p1, const Pose2d& p2)
{
return Pose2d(p1.x - p2.x, p1.y - p2.y, p1.phi - p2.phi);
}
Pose2d GraphicsView::getPosition()
{
QPointF pos = robot->pos();
double angle = robot->rotation();
return Pose2d(pos.x(), pos.y(), angle);
}
const Pose2d operator +(const Pose2d& p1, const Pose2d& p2)
{
return Pose2d(p1.x + p2.x, p1.y + p2.y, p1.phi + p2.phi);
}
QVariant PythonTypeConverter::toVariant(PyObject *input)
{
if(PyBool_Check(input))
return QVariant(input == Py_True);
if(PyLong_Check(input))
return QVariant((int)PyLong_AsLong(input));
if(PyFloat_Check(input))
return QVariant(PyFloat_AsDouble(input));
QString strVal = toString(input);
if(!strVal.isNull())
return strVal;
if(PyList_Check(input))
{
int len = PyList_Size(input);
QList<QVariant> list;
for(int i=0; i<len; i++)
list << toVariant(PyList_GetItem(input, i));
return bestFittingList(list);
}
if(PyTuple_Check(input))
{
int len = PyTuple_Size(input);
QList<QVariant> list;
for(int i=0; i<len; i++)
list << toVariant(PyTuple_GetItem(input, i));
return bestFittingList(list);
}
if(PyDict_Check(input))
{
QVariantMap map;
PyObject* key = 0;
PyObject* value = 0;
Py_ssize_t i = 0;
while (PyDict_Next(input, &i, &key, &value))
{
map.insert(toVariant(key).toString(), toVariant(value));
}
if(map.contains("type"))
{
QString type = map["type"].toString();
if(type == "Point")
return QPointF(map["x"].toDouble(), map["y"].toDouble());
if(type == "Size")
return QSizeF(map["width"].toDouble(), map["height"].toDouble());
if(type == "Rect")
return QRectF(map["x"].toDouble(), map["y"].toDouble(),map["width"].toDouble(), map["height"].toDouble());
if(type == "Vector3D")
return QVector3D(map["x"].toDouble(), map["y"].toDouble(),map["z"].toDouble());
if(type == "Vector4D")
return QVector4D(map["x"].toDouble(), map["y"].toDouble(),map["z"].toDouble(),map["w"].toDouble());
if(type == "Pose2d")
return QVariant::fromValue<Pose2d>(Pose2d(map["x"].toDouble(), map["y"].toDouble(),map["phi"].toDouble()));
}
return map;
}
return QVariant();
}
