bool EdgeSE2Segment2DLine::read(std::istream& is)
{
for (size_t i = 0; i < 2 ; i++)
is >> _measurement[i];
for (size_t i = 0; i < 2 ; i++)
for (size_t j = i; j < 2 ; j++) {
is >> _information (i,j);
_information (j,i) = _information (i,j);
}
return true;
}
bool EdgeSE2Segment2DLine::write(std::ostream& os) const
{
for (size_t i = 0; i < 2 ; i++)
os << _measurement[i] << " ";
for (size_t i = 0; i < 2 ; i++)
for (size_t j = i; j < 2 ; j++) {
os << _information (i,j) << " ";
}
return os.good();
}
static PyObject* information(PyObject* self, PyObject* args)
{
int i,len;
double tmp=0, n1=0, n2=0;
PyObject* obj1;
PyObject* obj2;
PyObject* seq1;
PyObject* seq2;
PyObject* item;
if (!PyArg_ParseTuple(args, "OO", &obj1, &obj2))
return NULL;
seq1 = PySequence_Fast(obj1, "expected a sequence");
seq2 = PySequence_Fast(obj2, "expected a sequence");
len = PySequence_Size(obj1);
double a1[len];
double a2[len];
for (i = 0; i < len; i++) {
item = PyList_GET_ITEM(seq1, i);
if (PyFloat_Check(item)){
tmp = PyFloat_AsDouble(item);
}
else if (PyInt_Check(item)){
tmp = (double) PyInt_AsLong(item);
}
a1[i]=tmp;
n1+=tmp;
item = PyList_GET_ITEM(seq2, i);
if (PyFloat_Check(item)){
tmp = PyFloat_AsDouble(item);
}
else if (PyInt_Check(item)){
tmp = (double) PyInt_AsLong(item);
}
a2[i]=tmp;
n2+=tmp;
}
Py_CLEAR(seq1);
Py_CLEAR(seq2);
return Py_BuildValue("d", _information(a1,a2,n1,n2,len));
}
SensorPointXYBearing::SensorPointXYBearing(const std::string& name_): BinarySensor<Robot2D, EdgeSE2PointXYBearing, WorldObjectPointXY>(name_) {
_information(0,0)=180.0 / M_PI;
}
