JNIEXPORT jobject JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_positionSearch (JNIEnv *env, jobject thisobject,
jobject startpos_object,
jobject map_object,
jobject scan_object,
jdouble sigma_xy_mm,
jdouble sigma_theta_degrees,
jint max_search_iter)
{
position_t startpos;
startpos.x_mm = get_double_field(env, startpos_object, "x_mm");
startpos.y_mm = get_double_field(env, startpos_object, "y_mm");
startpos.theta_degrees = get_double_field(env, startpos_object, "theta_degrees");
void * random = ptr_from_obj(env, thisobject);
position_t newpos =
rmhc_position_search(
startpos,
cmap_from_jmap(env, map_object),
cscan_from_jscan(env, scan_object),
sigma_xy_mm,
sigma_theta_degrees,
max_search_iter,
random);
jclass cls = (*env)->FindClass(env, "edu/wlu/cs/levy/breezyslam/components/Position");
jmethodID constructor = (*env)->GetMethodID(env, cls, "<init>", "(DDD)V");
jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees);
return newpos_object;
}
static ColorMap *
cmap_from_pyobject(PyObject *pyarray)
{
int len, i;
GradientColorMap *cmap;
len = PySequence_Size(pyarray);
if(len == 0)
{
PyErr_SetString(PyExc_ValueError,"Empty color array");
return NULL;
}
cmap = new(std::nothrow)GradientColorMap();
if(!cmap)
{
PyErr_SetString(PyExc_MemoryError,"Can't allocate colormap");
return NULL;
}
if(! cmap->init(len))
{
PyErr_SetString(PyExc_MemoryError,"Can't allocate colormap array");
delete cmap;
return NULL;
}
for(i = 0; i < len; ++i)
{
double left, right, mid, left_col[4], right_col[4];
int bmode, cmode;
PyObject *pyitem = PySequence_GetItem(pyarray,i);
if(!pyitem)
{
return NULL;
}
if(!get_double_field(pyitem, "left", &left) ||
!get_double_field(pyitem, "right", &right) ||
!get_double_field(pyitem, "mid", &mid) ||
!get_int_field(pyitem, "cmode", &cmode) ||
!get_int_field(pyitem, "bmode", &bmode) ||
!get_double_array(pyitem, "left_color", left_col, 4) ||
!get_double_array(pyitem, "right_color", right_col, 4))
{
return NULL;
}
cmap->set(i, left, right, mid,
left_col,right_col,
(e_blendType)bmode, (e_colorType)cmode);
Py_DECREF(pyitem);
}
return cmap;
}