/**************************************************
* method SpuriousAttractorIndex
*
* @return a number indicating how far we are from a
* spurious attractor
*
****************************************************/
float Reaching::SpuriousAttractorIndex()
{
joint_vec_t& da,tmp14,tmp24;
cart_vec_t& dx;
CMatrix4_t jac; //jacobian matrix
cart_vec_t& wxm1,tmp13;
float dalength;
v_sub(target,pos_cart,dx);
v4_sub(tar_angle,pos_angle,da);
Jacobian(pos_angle,jac);
inverse_diag3(weight_cart,wxm1);
m3_diag_v_multiply(wxm1,dx,tmp13);
m3_4_t_v_multiply(jac,tmp13,tmp14);
m4_diag_v_multiply(weight_angle,tmp14,tmp24);
v4_add(da,tmp24,tmp14);
dalength = v4_length(da);
if (dalength < 0.00001){
return 2;
}
else{
return (v4_length(tmp14))/dalength;
}
}
void particles_update(float timestep) {
int random = rand();
#ifndef CPU_ONLY
#ifndef OPEN_GL_CPU
kernel_memory_gl_aquire(k_particle_positions);
kernel_memory_gl_aquire(k_particle_velocities);
kernel_memory_gl_aquire(k_particle_lifetimes);
kernel_memory_gl_aquire(k_particle_randoms);
#endif
kernel_set_argument(k_update, 6, sizeof(cl_float), ×tep);
kernel_set_argument(k_update, 7, sizeof(cl_int), &reset);
kernel_set_argument(k_update, 8, sizeof(cl_int), &random);
kernel_run(k_update, particle_count);
reset = 0;
#ifndef OPEN_GL_CPU
kernel_memory_gl_release(k_particle_positions);
kernel_memory_gl_release(k_particle_velocities);
kernel_memory_gl_release(k_particle_lifetimes);
kernel_memory_gl_release(k_particle_randoms);
#endif
kernel_run_finish();
#else
for(int i = 0; i < particle_count; i++) {
particle_lifetimes[i] = particle_lifetimes[i] + timestep;
if ((particle_lifetimes[i] > 60.0) || ( v4_length(particle_velocities[i]) < 0.5 )) {
particle_lifetimes[i] = 0.0;
particle_positions[i] = v4(32,15,32,1);
int random_index = (random + i) % particle_count;
float rx = particle_randoms[random_index].x;
float ry = particle_randoms[random_index].y;
float rz = particle_randoms[random_index].z;
particle_velocities[i] = v4_mul(v4(rx, ry, rz, 0), 5);
} else {
/* Update positions and velocity */
particle_positions[i].x = particle_positions[i].x + (particle_velocities[i].x * timestep);
particle_positions[i].y = particle_positions[i].y + (particle_velocities[i].y * timestep);
particle_positions[i].z = particle_positions[i].z + (particle_velocities[i].z * timestep);
particle_velocities[i].y = particle_velocities[i].y - (9.81 * timestep);
/* Bounce on floors */
if (particle_positions[i].y < 15.0) {
particle_velocities[i].x = particle_velocities[i].x * 0.75;
particle_velocities[i].y = particle_velocities[i].y * 0.75;
particle_velocities[i].z = particle_velocities[i].z * 0.75;
particle_velocities[i].y = -particle_velocities[i].y;
}
}
}
#endif
}
inline int Reaching::HasStopped(){
return v4_length(v_angle)<zero_vel_angle;
}
