//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;
}
