static void game_update_view(float dt)
{
float dc = view_dc * (jump_b ? 2.0f * fabsf(jump_dt - 0.5f) : 1.0f);
float dx = view_ry * dt * 5.0f;
float k;
view_a += view_ry * dt * 90.f;
/* Center the view about the ball. */
v_cpy(view_c, file.uv->p);
v_inv(view_v, file.uv->v);
switch (config_get_d(CONFIG_CAMERA))
{
case 1: /* Camera 1: Viewpoint chases the ball position. */
v_sub(view_e[2], view_p, view_c);
break;
case 2: /* Camera 2: View vector is given by view angle. */
view_e[2][0] = fsinf(V_RAD(view_a));
view_e[2][1] = 0.f;
view_e[2][2] = fcosf(V_RAD(view_a));
dx = 0.0f;
break;
default: /* Default: View vector approaches the ball velocity vector. */
k = v_dot(view_v, view_v);
v_sub(view_e[2], view_p, view_c);
v_mad(view_e[2], view_e[2], view_v, k * dt / 4);
break;
}
/* Orthonormalize the basis of the view in its new position. */
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
/* Compute the new view position. */
k = 1.0f + v_dot(view_e[2], view_v) / 10.0f;
view_k = view_k + (k - view_k) * dt;
if (view_k < 0.5) view_k = 0.5;
v_cpy(view_p, file.uv->p);
v_mad(view_p, view_p, view_e[0], dx * view_k);
v_mad(view_p, view_p, view_e[1], view_dp * view_k);
v_mad(view_p, view_p, view_e[2], view_dz * view_k);
/* Compute the new view center. */
v_cpy(view_c, file.uv->p);
v_mad(view_c, view_c, view_e[1], dc);
/* Note the current view angle. */
view_a = V_DEG(fatan2f(view_e[2][0], view_e[2][2]));
}
void MoveR::IK4(const point& P)
{
//ROS_INFO("IK\n");
double th0=0,th1=0,th2=0,th3=0;
double x=P.x.data;
double y=P.y.data;
double z=P.z.data;
double p[3]={x,y,z};
double temp1=pow(x,2)+pow(y,2)+pow(z,2);
double norm_p=pow(temp1,0.5);
double b=acos((pow(lup,2)+pow(ldn,2)-pow(norm_p,2))/(2*lup*ldn));
th3=PI-b;
double a=asin((ldn/norm_p)*sin(b));
double s[3]={0,0,0};
double u_z[3]={0,0,1};
double p_s[3];
v_sub(p,s,p_s,3);
//ROS_INFO("p\n");
//v_print(p,3);
//ROS_INFO("s\n");
//v_print(s,3);
//ROS_INFO("p-s\n");
//v_print(p_s,3);
double norm_p_s=pow(pow(p_s[0],2)+pow(p_s[1],2)+pow(p_s[2],2),0.5);
double u_n[3]={p_s[0]/norm_p_s,p_s[1]/norm_p_s,p_s[2]/norm_p_s};
//ROS_INFO("u_n\n");
//v_print(u_n,3);
double tmp[3];
v_scalar_multip(v_dot(u_z,u_n,3),u_n,tmp,3);
double u[3],u_u[3];
//v_add(u_z,tmp,u,3);
//u correct on 2/17
v_cross(u_n,u_z,u);
double norm_u=pow(pow(u[0],2)+pow(u[1],2)+pow(u[2],2),0.5);
v_scalar_multip(1/norm_u,u,u_u,3);
//ROS_INFO("u_u \n");
//v_print(u_u,3);
double v[3],u_v[3];
v_cross(u_n,u_u,v);
double norm_v=pow(pow(v[0],2)+pow(v[1],2)+pow(v[2],2),0.5);
v_scalar_multip(1/norm_v,v,u_v,3);
//ROS_INFO("u_v \n");
//v_print(u_v,3);
double c[3];
v_scalar_multip(cos(a)*lup,u_n,tmp,3);
v_add(s,tmp,c,3);
double r=lup*sin(a);
//double fi=PI/2;
ROS_INFO("b %f\n",b);
ROS_INFO("a %f\n",a);
double tmp1[3],tmp2[3];
v_scalar_multip(cos(fi),u_u,tmp1,3);
v_scalar_multip(sin(fi),u_v,tmp2,3);
v_add(tmp1,tmp2,tmp,3);
v_scalar_multip(r,tmp,tmp,3);
double e[3];
v_add(c,tmp,e,3);
ROS_INFO("u_u \n");
v_print(u_u,3);
double ex=e[0],ey=e[1],ez=e[2];
ROS_INFO("ex %f\n",ex);
ROS_INFO("ey %f\n",ey);
ROS_INFO("ez %f\n",ez);
if(ex>=0&&ey<0)
th0=atan((double)abs(ex)/abs(ey));
else if(ex>=0&&ey>=0)
th0=PI-atan((double)abs(ex)/abs(ey));
else if(ex<0&&ey>=0)
th0=PI+atan((double)abs(ex)/abs(ey));
else if(ex<0&&ey<0)
th0=-atan((double)abs(ex)/abs(ey));
double temp2=pow(pow(ex,2)+pow(ey,2),0.5);
if(ez>=0)
th1=-atan((double)abs(ez)/temp2);
else if(ez<0)
th1=atan((double)abs(ez)/temp2);
//ROS_INFO("IK:th0 %f\n",th0);
//ROS_INFO("IK:th1 %f\n",th1);
//ROS_INFO("th3 %f\n",th3);
th2=atan2(-x*sin(th0)*sin(th1)+y*cos(th0)*sin(th1)-z*cos(th1),x*cos(th0)+y*sin(th0));
ROS_INFO("x %f y %f z %f\n",x,y,z);
ROS_INFO("th0 %f th1 %f th2 %f th3 %f fi% f\n",th0,th1,th2,th3,fi);
if(isnan(th0)||isnan(th1)||isnan(th2)||isnan(th3)||th0>TH0_MAX||th0<TH0_MIN||th1>TH1_MAX||th1<TH1_MIN||th3>TH3_MAX||th3<TH3_MIN){
ROS_INFO("Fail and dont move\n");
}
else{
ROS_INFO("go move move time=%f\n",move_time);
arm_angle.joint0.angle = th0;
arm_angle.joint0.duration = move_time;
arm_angle.joint1.angle = th1;
arm_angle.joint1.duration = move_time;
arm_angle.joint2.angle = th2;
arm_angle.joint2.duration = move_time;
arm_angle.joint3.angle = th3;
arm_angle.joint3.duration = move_time;
}
}
void game_draw(int pose, float st)
{
float fov = view_fov;
if (jump_b) fov *= 2.f * fabsf(jump_dt - 0.5);
if (game_state)
{
config_push_persp(fov, 0.1f, FAR_DIST);
glPushMatrix();
{
float v[3], rx, ry;
float pup[3];
float pdn[3];
v_cpy(pup, view_p);
v_cpy(pdn, view_p);
pdn[1] = -pdn[1];
/* Compute and apply the view. */
v_sub(v, view_c, view_p);
rx = V_DEG(fatan2f(-v[1], fsqrtf(v[0] * v[0] + v[2] * v[2])));
ry = V_DEG(fatan2f(+v[0], -v[2])) + st;
glTranslatef(0.f, 0.f, -v_len(v));
glRotatef(rx, 1.f, 0.f, 0.f);
glRotatef(ry, 0.f, 1.f, 0.f);
glTranslatef(-view_c[0], -view_c[1], -view_c[2]);
#ifndef DREAMCAST_KGL_NOT_IMPLEMENT
if (config_get_d(CONFIG_REFLECTION))
{
/* Draw the mirror only into the stencil buffer. */
glDisable(GL_DEPTH_TEST);
glEnable(GL_STENCIL_TEST);
glStencilFunc(GL_ALWAYS, 1, 0xFFFFFFFF);
glStencilOp(GL_REPLACE, GL_REPLACE, GL_REPLACE);
glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
game_refl_all(0);
/* Draw the scene reflected into color and depth buffers. */
glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
glStencilFunc(GL_EQUAL, 1, 0xFFFFFFFF);
glEnable(GL_DEPTH_TEST);
glFrontFace(GL_CW);
glPushMatrix();
{
glScalef(+1.f, -1.f, +1.f);
game_draw_light();
game_draw_back(pose, -1, pdn);
game_draw_fore(pose, rx, ry, -1, pdn);
}
glPopMatrix();
glFrontFace(GL_CCW);
glDisable(GL_STENCIL_TEST);
}
#endif
/* Draw the scene normally. */
game_draw_light();
game_refl_all(pose ? 0 : config_get_d(CONFIG_SHADOW));
game_draw_back(pose, +1, pup);
game_draw_fore(pose, rx, ry, +1, pup);
}
glPopMatrix();
config_pop_matrix();
/* Draw the fade overlay. */
fade_draw(fade_k);
}
}
viennacl::vector_range<viennacl::vector<T> >
dynVCLVec<T>::data() {
viennacl::vector_range<viennacl::vector<T> > v_sub(*ptr, r);
return v_sub;
}
