rs2_pose pose_multiply(const rs2_pose& ref2_in_ref1, const rs2_pose& ref3_in_ref2)
{
rs2_pose ref3_in_ref1;
ref3_in_ref1.rotation = quaternion_multiply(ref2_in_ref1.rotation, ref3_in_ref2.rotation);
ref3_in_ref1.translation = vector_addition(quaternion_rotate_vector(ref2_in_ref1.rotation, ref3_in_ref2.translation), ref2_in_ref1.translation);
return ref3_in_ref1;
}
// equation (4) from the paper
HRESULT ParticleSwarmOptimizer::update_position( Particle& p ) {
HRESULT hr = S_OK;
p.curr_sln = vector_addition(p.curr_sln, p.velocity);
return hr;
}
// equation (3) from the paper
HRESULT ParticleSwarmOptimizer::update_velocity( Particle& p ) {
HRESULT hr = S_OK;
vector<float> cog_comp; // individual cognitive component
vector<float> soc_comp; // social component
cog_comp = vector_multiply(constriction_factor,
vector_addition(p.velocity,
vector_multiply(cognitive_weight*cognitive_random_factor,
vector_difference(p.best_sln,
p.curr_sln
))));
soc_comp = vector_multiply(social_weight*social_random_factor,
vector_difference(best_sln,
p.curr_sln
));
p.velocity = vector_addition(cog_comp, soc_comp);
return hr;
}
double *projection(Matrix *m, double *v, int length){
unsigned int i, j;
double *sum, *copy, *vector, factor;
if(m->rows != length)
return NULL;
if(m == NULL || v == NULL)
return NULL;
sum = calloc(sizeof(double), m->rows);
copy = malloc(sizeof(double)*m->rows);
for(i = 0; i < m->columns; i++){
for(j = 0; j < m->rows; j++)
copy[j] = m->numbers[i][j];
vector = copy;
factor = vector_multiply(v, vector, m->rows)/vector_multiply(vector, vector, m->rows);
scalar_vector_multiplication(factor, vector, m->rows);
vector_addition(sum, vector, m->rows);
}
free(copy);
return sum;
}
rs2_vector pose_transform_point(const rs2_pose& pose, const rs2_vector& p)
{
return vector_addition(quaternion_rotate_vector(pose.rotation, p), pose.translation);
}