// works for homogeneous weights only. not clean
float Reaching::GetCosts(joint_vec_t& des_angle,cart_vec_t& des_pos){
joint_vec_t& v1,v2;
cart_vec_t& w1,w2;
float s1,s2;
v4_sub(des_angle,pos_angle,v1);
//m4_diag_v_multiply(weight_angle,v1,v2);
if (weight_angle[0] == 0){
s1 = v4_dot(v1,v1);
return s1;
}
else{
v4_scale(v1,1/weight_angle[0],v2);
s1 = v4_dot(v1,v2);
}
v_sub(des_cart,pos_cart,w1);
if (weight_cart[0] == 0){
s2 = v_dot(w1,w1);
return s2;
}
else{
v4_scale(w1,1/weight_cart[0],w2);
s2 = v_dot(w1,w2);
}
// m3_diag_v_multiply(weight_cart,w1,w2);
return (s1+s2)/(1/weight_cart[0]+1/weight_angle[0]);
}
/** Catmull-rom spline interpolation */
scalar_t spline(scalar_t a, scalar_t b, scalar_t c, scalar_t d, scalar_t t)
{
vec4_t tmp;
scalar_t tsq = t * t;
static mat4_t crspline_mat = {
{-1, 3, -3, 1},
{2, -5, 4, -1},
{-1, 0, 1, 0},
{0, 2, 0, 0}
};
tmp = v4_scale(v4_transform(v4_cons(a, b, c, d), crspline_mat), 0.5);
return v4_dot(v4_cons(tsq * t, tsq, t, 1.0), tmp);
}
/** b-spline approximation */
scalar_t bspline(scalar_t a, scalar_t b, scalar_t c, scalar_t d, scalar_t t)
{
vec4_t tmp;
scalar_t tsq = t * t;
static mat4_t bspline_mat = {
{-1, 3, -3, 1},
{3, -6, 3, 0},
{-3, 0, 3, 0},
{1, 4, 1, 0}
};
tmp = v4_scale(v4_transform(v4_cons(a, b, c, d), bspline_mat), 1.0 / 6.0);
return v4_dot(v4_cons(tsq * t, tsq, t, 1.0), tmp);
}
