//nearest points between two segments given by pi+veci, results given in ratio of vec [0 1]
int Reaching::FindSegmentsNearestPoints(cart_vec_t& p1,cart_vec_t& vec1,cart_vec_t& p2,cart_vec_t& vec2, float *nearest_point1, float *nearest_point2, float *dist){
CMatrix3_t mat;
CMatrix3_t invmat;
cart_vec_t& vec3,tmp,tmp1,tmp2,k,v2;
int i;
m_identity(mat);
v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not //
if(v_squ_length(vec3)<0.00001){
return 0;
}
v_scale(vec2,-1,v2);
m_set_v3_column(vec1,0,mat);
m_set_v3_column(v2,1,mat);
m_set_v3_column(vec3,2,mat);
m_inverse(mat,invmat);
v_sub(p2,p1,tmp);
v_transform_normal(tmp,invmat,k);
for(i=0;i<2;i++){
k[i] = max(min(1,k[i]),0);
}
v_scale(vec1,k[0],tmp);
v_add(p1,tmp,tmp1);
v_scale(vec2,k[1],tmp);
v_add(p2,tmp,tmp2);
v_sub(tmp2,tmp1,tmp);
*dist=v_length(tmp);
*nearest_point1 = k[0];
*nearest_point2 = k[1];
return 1;
}
void
pl_area_vec(const Polygon * pl, vec dest)
{
assert(pl);
v_zero(dest);
if (pl->last < 3) return;
uint i, j;
vec tmp;
for (i=pl->last - 1, j=0; j < pl->last; i=j++){
v_cross(tmp, pl->points[i].co, pl->points[j].co);
v_add(dest, dest, tmp);
}
}
int main(int argc, char **argv)
{
COORD cord[3];
float a,b;
scanf("%f %f %f %f %f %f %f", &cord[0].x, &cord[0].y, &cord[0].z, &cord[1].x, &cord[1].y, &cord[1].z, &a);
printf("\nv1 = ( %f , %f , %f)", cord[0].x, cord[0].y, cord[0].z);
printf("\nv2 = ( %f , %f , %f)", cord[1].x, cord[1].y, cord[1].z);
printf("\na = %f ", a);
cord[2] = v_sum(&cord[0],&cord[1]);
printf("\nv1+v2 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
cord[2] = v_dif(&cord[0],&cord[1]);
printf("\nv1-v2 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
b = v_dot(&cord[0],&cord[1]);
printf("\nv1.v2 = %f ", b);
cord[2] = v_prod(&a,&cord[0]);
printf("\na v1 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
cord[2] = v_cross(&cord[0],&cord[1]);
printf("\nv1 x v2 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
b = v_mod_2(&cord[0]);
printf("\nv1^2 = %f ", b);
b = v_mod(&cord[0]);
printf("\n|v1| = %f ", b);
printf("\n\n");
return(0);
}
/* Third version of draw triangle uses a z_buffer
And we're using flat shading
clear_z_buffer needs to be run in the main loop before every frame. */
void draw_triangle3(int *a,int *b,int *c,vect_t pos,vect_t rot,rgb color){
int x0,y0,x1,y1,x2,y2;
// this is is the format that the triangles should be passed to us already
vect_t x={a[0],a[1],a[2]+Z};
vect_t y={b[0],b[1],b[2]+Z};
vect_t z={c[0],c[1],c[2]+Z};
// Lighting
vect_t n=v_norm(v_cross(x,y));
// vect_t l=v_sub(cam.pos,x);
vect_t l=cam.pos;
double lum=1.0;
// if( lum < 0 ) return 0;
rgb colors[3];
point p[3];
x=v_rot(x,rot);
y=v_rot(y,rot);
z=v_rot(z,rot);
// should we do the translation here as well does this make sense?
x=v_add(x,pos);
y=v_add(y,pos);
z=v_add(z,pos);
double lums[3];
lums[0]=v_dot(l,x)/(v_len(l)*v_len(x));
lums[1]=v_dot(l,y)/(v_len(l)*v_len(y));
lums[2]=v_dot(l,z)/(v_len(l)*v_len(z));
#if 1
for(int i=0;i<3;i++){
#if 0
lum=lums[i];
if( lum < 0 ) lum=0;
#else
lum=1.0;
#endif
colors[i].r=clip(color.r*lum);
colors[i].g=clip(color.g*lum);
colors[i].b=clip(color.b*lum);
}
#else
for(int i=0;i<3;i++){
colors[i].r=color.r;
colors[i].g=color.g;
colors[i].b=color.b;
}
#endif
int q,w,e;
q=project_coord(x,&p[0].x,&p[0].y);
w=project_coord(y,&p[1].x,&p[1].y);
e=project_coord(z,&p[2].x,&p[2].y);
if(outside_of_screen(p)){
return;
}
rgb ca[3];
ca[0]=ca[1]=ca[2]=color;
vect_t v3[3];
v3[0]=x; v3[1]=y; v3[2]=z;
double distances[3];
// introduce max distance of 5000
distances[0]=clipfloat(v_len(v_sub(x,cam.pos)),0,MAX_DIST)/MAX_DIST;
distances[1]=clipfloat(v_len(v_sub(y,cam.pos)),0,MAX_DIST)/MAX_DIST;
distances[2]=clipfloat(v_len(v_sub(z,cam.pos)),0,MAX_DIST)/MAX_DIST;
/*
distances[0]=v_len(v_sub(z,cam.pos));
distances[1]=v_len(v_sub(y,cam.pos));
distances[2]=v_len(v_sub(x,cam.pos));
*/
draw_triangle_gradient_new(p,v3,distances,colors);
#if 0
draw_line_color(p[0].x,p[0].y,p[1].x,p[1].y,black);
draw_line_color(p[1].x,p[1].y,p[2].x,p[2].y,black);
draw_line_color(p[1].x,p[1].y,p[0].x,p[0].y,black);
#endif
}
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;
}
}
