//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 sm_frame(sm_t *sm,int from,int to,float frame) { int i,frame0,frame1; if(from == -1) from = 0; if(to == -1) to = sm->num_frame; frame0 = (int)frame; frame -= frame0; frame0 += from; if(frame0 >= to) frame0 = ((frame0 - from) % (to - from)) + from; frame1 = frame0 + 1; if(frame1 >= to) frame1 = from; for(i = 0; i < sm->num_bone; i++) { float m0[16],m1[16]; float xyz0[3],xyz1[3],xyz[3],rot[4]; v_scale(sm->frame[frame0][i].xyz,1.0 - frame,xyz0); v_scale(sm->frame[frame1][i].xyz,frame,xyz1); v_add(xyz0,xyz1,xyz); q_slerp(sm->frame[frame0][i].rot,sm->frame[frame1][i].rot,frame,rot); m_translate(xyz,m0); q_to_matrix(rot,m1); m_multiply(m0,m1,sm->bone[i].matrix); } for(i = 0; i < sm->num_surface; i++) { int j; sm_surface_t *s = sm->surface[i]; for(j = 0; j < s->num_vertex; j++) { int k; v_set(0,0,0,s->vertex[j].xyz); v_set(0,0,0,s->vertex[j].normal); for(k = 0; k < s->vertex[j].num_weight; k++) { float v[3]; sm_weight_t *w = &s->vertex[j].weight[k]; v_transform(w->xyz,sm->bone[w->bone].matrix,v); v_scale(v,w->weight,v); v_add(s->vertex[j].xyz,v,s->vertex[j].xyz); v_transform_normal(w->normal,sm->bone[w->bone].matrix,v); v_scale(v,w->weight,v); v_add(s->vertex[j].normal,v,s->vertex[j].normal); } } } }
int Reaching::LinkDistanceToObject(int link, EnvironmentObject *obj, float *dist, float *point, cart_vec_t& contact_vector){ cart_vec_t& objPos; cart_vec_t& lowerLinkExtr; cart_vec_t& upperLinkExtr; cart_vec_t& v1,v2,linkv,u,vertexPos,v_tmp,tmp3; CMatrix4_t ref,tmpmat,tmpmat2; cart_vec_t& s1,s2; int i,j,k,dir1,dir2,pindex,min_i; float alldists[12]; //closest distance to each edge float allpoints[24]; // closest pair of points on each edge float min_dist; int s3[3]; for (i=0;i<12;i++){ alldists[i]=FLT_MAX; } switch(link){ case 1: v_clear(upperLinkExtr); ElbowPosition(pos_angle,lowerLinkExtr); break; case 2: ElbowPosition(pos_angle,upperLinkExtr); v_copy(pos_cart,lowerLinkExtr); break; default: cout<<"unvalid link number"<< endl; } if(!obj){ return 0; } // v_sub(obj->solid->m_position, shoulder_abs_pos,objPos); Abs2LocalRef(obj->GetPosition(), objPos); // cout<<"ule: ";coutvec(upperLinkExtr); // cout<<"lle: ";coutvec(lowerLinkExtr); // coutvec(objPos); v_sub(lowerLinkExtr,objPos,v1); v_sub(upperLinkExtr,objPos,v2); //m_transpose(obj->solid->m_ref.m_orient,ref); //stupid to do it each time m_copy(obj->solid->m_ref.m_orient,ref); //stupid to do it each time v_clear(s3); //checking when two parallel sides must me checked for(i=0;i<3;i++){ s1[i] = v_dot(v1, &ref[i*4]); s2[i] = v_dot(v2, &ref[i*4]); if (s1[i]*s2[i]>=0){ s3[i] = sign(s1[i]+s2[i]); } } // cout << "s3: ";coutvec(s3); m_copy(ref,tmpmat); for(i=0;i<3;i++){ if(s3[i]){ v_sub(lowerLinkExtr,upperLinkExtr,linkv); v_scale(obj->solid->m_size,-0.5,u); u[i] *=-s3[i]; v_add(objPos,u,vertexPos); // cout<<"vPos "<<i<<": ";coutvec(vertexPos); v_scale(&(ref[i*4]),s3[i],&(tmpmat[i*4])); // cout<<"norm "<<i<<": ";coutvec((tmpmat+i*4)); m_inverse(tmpmat,tmpmat2); if(v_dot(&(tmpmat[i*4]),linkv)<0){ v_sub(lowerLinkExtr,vertexPos,v_tmp); *point = 1; } else{ v_sub(upperLinkExtr,vertexPos,v_tmp); *point = 0; } // cout<<"linkv ";coutvec(linkv); // cout <<"pt "<<*point<<endl; // #ifdef OLD_AV // v_copy(linkv,(cart_vec_t&)&tmpmat[i*4]); // m_inverse(tmpmat,tmpmat2); // v_sub(upperLinkExtr,vertexPos,tmp2); // tmp3 should contain the intersection coordinates in // the referential defined by the edges of the surface v_transform_normal(v_tmp,tmpmat2,tmp3); // cout<<"tmp3 ";coutvec(tmp3); if(tmp3[(i+1)%3]>=0 && tmp3[(i+2)%3]>=0 // the link points to the rectangle && tmp3[(i+1)%3]<=obj->solid->m_size[(i+1)%3] && tmp3[(i+2)%3]<=obj->solid->m_size[(i+2)%3]){ if(tmp3[i]<0){ return 0; // there is a collision } else{ *dist = tmp3[i]; v_copy(&(tmpmat[i*4]),contact_vector); v_scale(contact_vector,*dist,contact_vector); return 1; } } } } // the link does not point to a rectangle -> look for the edges v_scale(obj->solid->m_size,-0.5,u); for(i=0;i<3;i++){// each kind of edge dir1 = s3[(i+1)%3]; dir2 = s3[(i+2)%3]; for(j=0;j<2;j++){ if(dir1 == 0 || dir1==2*j-1){ //edges of this face must be computed for(k=0;k<2;k++){ if(dir2 == 0 || dir2==2*k-1){ //edges of this face must be computed v_copy(u,v_tmp); v_tmp[(i+1)%3]*=-(2*j-1); v_tmp[(i+2)%3]*=-(2*k-1); v_add(objPos,v_tmp,vertexPos); v_scale(&(ref[4*i]),obj->solid->m_size[i],v1); // edge with the right length pindex = 4*i+2*j+k; FindSegmentsNearestPoints(vertexPos,v1,upperLinkExtr,linkv,&(allpoints[2*pindex]),&(allpoints[2*pindex+1]),&(alldists[pindex])); // cout<<"i:"<<i<<" j:"<<j<<" k:"<<k<<"dist "<<alldists[pindex]<<endl; // cout<<"v1:";coutvec(v1); // cout<<"ref:";coutvec((&(ref[4*i]))); // cout<<"vP:";coutvec(vertexPos); } } } } } //looking for the min min_dist = alldists[0]; min_i = 0; for(i=1;i<12;i++){ if(alldists[i] < min_dist){ min_dist = alldists[i]; min_i = i; } } // returning the min distance *dist = min_dist; *point = allpoints[2*min_i+1]; v_scale(linkv,*point,v1); v_add(upperLinkExtr,v1,v2); // nearest point on link k = min_i%2; //retrieving the right edge j = ((min_i-k)%4)/2; i = min_i/4; // cout<<"ijk: "<<i<<" "<<j<<" "<<k<<endl; v_copy(u,v_tmp); v_tmp[(i+1)%3]*=-(2*j-1); v_tmp[(i+2)%3]*=-(2*k-1); v_add(objPos,v_tmp,vertexPos); // starting vertex of the edge v_scale(&(ref[3*1]),allpoints[2*min_i]*(obj->solid->m_size[min_i]),v1); v_add(vertexPos,v1,v1); // nearest point on solid v_sub(v2,v1,contact_vector); return 1; }