hMatrix Inverse_Kinematics(hMatrix Initial_T,hMatrix Goal_T,double *Initial_t, double *DH_alpha, double *DH_a, double *DH_d, int joint){
for(int i=0; i<joint; i++){
Initial_theta[i] = *Initial_t;
Initial_t++;
}
hMatrix Initial_Theta(7,1);
hMatrix J(6,7), Pinv_J(7,6);
hMatrix n_a(3,1),s_a(3,1),a_a(3,1),n_t(3,1),s_t(3,1),a_t(3,1),p_del(3,1);
double x,y,z,rx,ry,rz;
double error_position[3]= {Goal_T.element(0,3)-Initial_T.element(0,3),Goal_T.element(1,3)-Initial_T.element(1,3),Goal_T.element(2,3)-Initial_T.element(2,3)};
hMatrix P(3,1),R(3,1),Rotation(3,3),dx_temp1(3,1),dx_temp2(3,1),dX(6,1),del_Theta(7,1),Temp(7,1);
Initial_Theta.SET(7,1,Initial_theta);
Initial_T = T_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0], joint);
J = Jacobian_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0]);
Pinv_J = Pseudo_Inverse(J);
for(int i = 0; i<3; i++){
n_a.SetElement(i,0,Initial_T.element(i,0));
s_a.SetElement(i,0,Initial_T.element(i,1));
a_a.SetElement(i,0,Initial_T.element(i,2));
n_t.SetElement(i,0,Goal_T.element(i,0));
s_t.SetElement(i,0,Goal_T.element(i,1));
a_t.SetElement(i,0,Goal_T.element(i,2));
p_del.SetElement(i,0,Goal_T.element(i,3)-Initial_T.element(i,3));
}
x = dot(n_a, p_del);
y = dot(s_a, p_del);
z = dot(a_a, p_del); ;
rx = (dot(a_a,s_t)-dot(a_t,s_a))/2;
ry = (dot(n_a,a_t)-dot(n_t,a_a))/2;
rz = (dot(s_a,n_t)-dot(s_t,n_a))/2;
double dx_P[3] = {x,y,z},dx_R[3] = {rx,ry,rz};
P.SET(3,1,&dx_P[0]);
R.SET(3,1,&dx_R[0]);
Rotation = T_Rotation(Initial_T);
dx_temp1 = Rotation*P;
dx_temp2 = Rotation*R;
for(int i =0; i<3; i++){
dX.SetElement(i,0,dx_temp1.element(i,0));
dX.SetElement(i+3,0,dx_temp2.element(i,0));
}
del_Theta = Pinv_J*dX;
for(int i=0; i<joint; i++)
Temp.SetElement(i,0,Initial_Theta.element(i,0) + del_Theta.element(i,0));
Initial_Theta = Temp;
return Initial_Theta;
}
_table_hdr_t::n_t _table_hdr_t::fetch_n(std::istream &stream) const
{
std::vector<point> v(fetch_32(stream));
for(std::size_t i = 0; i < v.size(); ++i)
{
coord_t x = fetch_8(stream);
coord_t y = fetch_8(stream);
v[i] = point(x, y);
}
return n_t(std::move(v));
}
void PolarCode::initialize_frozen_bits() {
std::vector<double> channel_vec(_block_length);
for (uint16_t i = 0; i < _block_length; ++i) {
channel_vec.at(i) = _design_epsilon;
}
for (uint8_t iteration = 0; iteration < _n; ++iteration) {
uint16_t increment = 1 << iteration;
for (uint16_t j = 0; j < increment; j += 1) {
for (uint16_t i = 0; i < _block_length; i += 2 * increment) {
double c1 = channel_vec.at(i + j);
double c2 = channel_vec.at(i + j + increment);
channel_vec.at(i + j) = c1 + c2 - c1*c2;
channel_vec.at(i + j + increment) = c1*c2;
}
}
}
_channel_order_descending.resize(_block_length);
std::size_t n_t(0);
std::generate(std::begin(_channel_order_descending), std::end(_channel_order_descending), [&]{ return n_t++; });
std::sort( std::begin(_channel_order_descending),
std::end(_channel_order_descending),
[&](int i1, int i2) { return channel_vec[_bit_rev_order.at(i1)] < channel_vec[_bit_rev_order.at(i2)]; } );
uint16_t effective_info_length = _info_length + _crc_size;
for (uint16_t i = 0; i < effective_info_length; ++i) {
_frozen_bits.at(_channel_order_descending.at(i)) = 0;
}
for (uint16_t i = effective_info_length; i < _block_length; ++i) {
_frozen_bits.at(_channel_order_descending.at((i))) = 1;
}
_crc_matrix.resize(_crc_size);
for (uint8_t bit = 0; bit < _crc_size; ++bit) {
_crc_matrix.at(bit).resize(_info_length);
for (uint16_t info_bit = 0; info_bit < _info_length; ++info_bit )
_crc_matrix.at(bit).at(info_bit) = (uint8_t) (rand() % 2);
}
}
