// compute the vector VEC between a 3D point P0 and a line (
// passing through two 3D points P and S)
double vec_pt_to_line3D(double *P,double *S,double *P0,double *VEC)
{
double diff10[3];
double diff21[3];
double dot_prod_diff10_diff21;
double t,norm_diff21;
double VT[3];
double diffVT0[3];
double norm_diffVT0;
VEC_DIFF(diff10,P,P0);
VEC_DIFF(diff21,S,P);
VEC_LENGTH(norm_diff21,diff21);
VEC_DOT_PRODUCT(dot_prod_diff10_diff21,diff10,diff21);
t=-dot_prod_diff10_diff21 / ( norm_diff21*norm_diff21 );
VEC_COPY(VT,P)
VEC_ACCUM(VT,t,diff21);
VEC_DIFF(diffVT0,VT,P0);
for(int t=0;t<3;t++)
{
VEC[t] = P0[t];
VEC[t+3] = VT[t];
}
VEC_LENGTH(norm_diffVT0,diffVT0);
return norm_diffVT0;
}
/*
* The uviewpoint subroutine computes and returns a 4x4 matrix that
* translates the origen to the point v1, puts the negative z axis
* along the direction v21==v2-v1, and puts the y axis along the up
* vector.
*/
void uviewpoint (gleDouble m[4][4], /* returned */
gleDouble v1[3], /* input */
gleDouble v2[3], /* input */
gleDouble up[3]) /* input */
{
gleDouble v_hat_21 [3];
gleDouble trans_mat[4][4];
gleDouble rot_mat[4][4];
/* find the vector that points in the v21 direction */
VEC_DIFF (v_hat_21, v2, v1);
/* compute rotation matrix that takes -z axis to the v21 axis,
* and y to the up dierction */
uview_direction (rot_mat, v_hat_21, up);
/* build matrix that translates the origin to v1 */
IDENTIFY_MATRIX_4X4 (trans_mat);
trans_mat[3][0] = v1[0];
trans_mat[3][1] = v1[1];
trans_mat[3][2] = v1[2];
/* concatenate the matrices together */
MATRIX_PRODUCT_4X4 (m, rot_mat, trans_mat);
}
void kalman_update_3x3(kalman_state_n* state, float* measurement,float** A, float** H)
{
float** auxMat;
auxMat=(float**)malloc(3*sizeof(float*));
for (int i=0; i<3; i++) auxMat[i]=malloc(3*sizeof(float));
float* auxVet;
auxVet=(float*)malloc(3*sizeof(float));
float pTrsp[3][3],Atrsp[3][3];
/*predicted p*/
TRANSPOSE_MATRIX_3X3(Atrsp, A);
MATRIX_PRODUCT_3X3(auxMat, state->p, Atrsp);
MATRIX_PRODUCT_3X3(state->p, A, auxMat);
ACCUM_SCALE_MATRIX_3X3(state->p, 1, state->q)
/*predicted x*/
MAT_DOT_VEC_3X3(state->x, A, state->x);
// state->x=xaux;
/*kalman gain*/
float Htrsp[3][3], S[3][3], Sinv[3][3], B[3][3], det;
TRANSPOSE_MATRIX_3X3(Htrsp, H);
TRANSPOSE_MATRIX_3X3(pTrsp, state->p);
MATRIX_PRODUCT_3X3(auxMat, pTrsp, Htrsp);
MATRIX_PRODUCT_3X3(S, H, auxMat);
ACCUM_SCALE_MATRIX_3X3(S, 1.0, state->r);
MATRIX_PRODUCT_3X3(B, H, pTrsp);
INVERT_3X3(Sinv, det, S);
MATRIX_PRODUCT_3X3(auxMat, Sinv, B);
TRANSPOSE_MATRIX_3X3(state->k, auxMat);
// MAT_PRINT_3X3(state->k);
/*estimated x*/
MAT_DOT_VEC_3X3(auxVet, H, state->x);
VEC_DIFF(auxVet, measurement, auxVet);
MAT_DOT_VEC_3X3(auxVet, state->k, auxVet);
VEC_SUM(state->x, state->x, auxVet);
/*estimated P*/
MATRIX_PRODUCT_3X3(auxMat, H, state->p);
MATRIX_PRODUCT_3X3(auxMat, state->k, auxMat);
ACCUM_SCALE_MATRIX_3X3(state->p, -1.0, auxMat);
/*out*/
MAT_DOT_VEC_3X3(measurement, H, state->x);
for (int i=0; i<3; i++) free(auxMat[i]);
free(auxMat);
free(auxVet);
}
/*!
\param triangle
\param s1
\param s2
\param contacts Contains the closest points on the segment (1,2), and the normal points to segment, and m_depth contains the distance
\post The contacts array is not set to 0. It adds aditional contacts
*/
void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,vec3f s2, GDYNAMIC_ARRAY * contacts)
{
vec3f segment_points[4];
vec3f closest_points[2];
GUINT intersection_type, out_edge= 10;
GREAL dis, dis_temp,perpend;
vec4f sdiff;
dis = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s1);
dis_temp = DISTANCE_PLANE_POINT(triangle->m_planes.m_planes[0],s2);
if(dis<=0.0f && dis_temp<=0.0f) return;
VEC_DIFF(sdiff,s2,s1);
perpend = VEC_DOT(sdiff,triangle->m_planes.m_planes[0]);
if(!IS_ZERO(perpend)) // Not perpendicular
{
if(dis<dis_temp)
{
VEC_COPY(closest_points[0],s1);
}
else
{
dis = dis_temp;
VEC_COPY(closest_points[0],s2);
}
//Testing segment vertices over triangle
if(dis>=0.0f && dis_temp>=0.0f)
{
POINT_IN_HULL(closest_points[0],(&triangle->m_planes.m_planes[1]),3,out_edge);
if(out_edge==0)//Point over face
{
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
return;
}
}
else
{
PLANE_CLIP_SEGMENT(s1,s2,triangle->m_planes.m_planes[0],closest_points[1]);
POINT_IN_HULL(closest_points[1],(&triangle->m_planes.m_planes[1]),3,out_edge);
if(out_edge==0)//Point over face
{
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
return;
}
}
}
else // Perpendicular Face
{
//out_edge=10
//Clip segment by triangle
// Edge1
PLANE_CLIP_SEGMENT_CLOSEST(s1,s2,triangle->m_planes.m_planes[1],segment_points[0],segment_points[1],intersection_type);
if(intersection_type==0||intersection_type==1)
{
out_edge = 0;
VEC_COPY(closest_points[0],segment_points[0]);
}
else
{
//Edge2
PLANE_CLIP_SEGMENT_CLOSEST(segment_points[0],segment_points[1],triangle->m_planes.m_planes[2],segment_points[2],segment_points[3],intersection_type);
if(intersection_type==0||intersection_type==1)
{
out_edge = 1;
VEC_COPY(closest_points[0],segment_points[3]);
}
else
{
//Edge3
PLANE_CLIP_SEGMENT_CLOSEST(segment_points[2],segment_points[3],triangle->m_planes.m_planes[3],closest_points[0],closest_points[1],intersection_type);
if(intersection_type==0||intersection_type==1)
{
out_edge = 2;
}
}
}
//POST closest_points[0] and closest_points[1] are inside the triangle, if out_edge>2
if(out_edge>2) // Over triangle
{
dis = VEC_DOT(closest_points[0],triangle->m_planes.m_planes[0]);
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
GIM_PUSH_CONTACT((*contacts),closest_points[1] ,triangle->m_planes.m_planes[0] ,dis,0, 0, 0,0);
return;
}
}
//Find closest edges
out_edge = 10;
dis = G_REAL_INFINITY;
GUINT i;
for(i=0;i<3;i++)
{
SEGMENT_COLLISION(s1,s2,triangle->m_vertices[i],triangle->m_vertices[(i+1)%3],segment_points[0],segment_points[1]);
VEC_DIFF(sdiff,segment_points[0],segment_points[1]);
dis_temp = VEC_DOT(sdiff,sdiff);
if(dis_temp< dis)
{
dis = dis_temp;
out_edge = i;
VEC_COPY(closest_points[0],segment_points[0]);
VEC_COPY(closest_points[1],sdiff);//normal
}
}
if(out_edge>2) return ;// ???? ASSERT this please
if(IS_ZERO(dis))
{
//Set face plane
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,triangle->m_planes.m_planes[0] ,0.0f,0, 0, 0,0);
}
else
{
GIM_SQRT(dis,dis);
VEC_SCALE(closest_points[1],(1.0f/dis),closest_points[1]);//normal
GIM_PUSH_CONTACT((*contacts),closest_points[0] ,closest_points[1],dis,0, 0, 0,0);
}
}
int gim_triangle_sphere_collision(
GIM_TRIANGLE_DATA *tri,
vec3f center, GREAL radius,
GIM_TRIANGLE_CONTACT_DATA * contact_data)
{
contact_data->m_point_count = 0;
//Find Face plane distance
GREAL dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[0],center);
if(dis>radius) return 0; //out
if(dis<-radius) return 0;//Out of triangle
contact_data->m_penetration_depth = dis;
//Find the most edge
GUINT32 most_edge = 4;//no edge
GREAL max_dis = 0.0f;
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[1],center);
if(dis>radius) return 0;//Out of triangle
if(dis>0.0f)
{
max_dis = dis;
most_edge = 0;
}
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[2],center);
if(dis>radius) return 0;//Out of triangle
if(dis>max_dis)// && dis>0.0f)
{
max_dis = dis;
most_edge = 1;
}
dis = DISTANCE_PLANE_POINT(tri->m_planes.m_planes[3],center);
if(dis>radius) return 0;//Out of triangle
if(dis>max_dis)// && dis>0.0f)
{
max_dis = dis;
most_edge = 2;
}
if(most_edge == 4) //Box is into triangle
{
//contact_data->m_penetration_depth = dis is set above
//Find Face plane point
VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[0]);
//Find point projection on plane
if(contact_data->m_penetration_depth>=0.0f)
{
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
}
else
{
VEC_SCALE(contact_data->m_points[0],radius,contact_data->m_separating_normal);
}
contact_data->m_penetration_depth = radius - contact_data->m_penetration_depth;
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
//Scale normal for pointing to triangle
VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
contact_data->m_point_count = 1;
return 1;
}
//find the edge
vec3f e1,e2;
VEC_COPY(e1,tri->m_vertices[most_edge]);
VEC_COPY(e2,tri->m_vertices[(most_edge+1)%3]);
CLOSEST_POINT_ON_SEGMENT(contact_data->m_points[0],center,e1,e2);
//find distance
VEC_DIFF(e1,center,contact_data->m_points[0]);
VEC_LENGTH(e1,dis);
if(dis>radius) return 0;
contact_data->m_penetration_depth = radius - dis;
if(IS_ZERO(dis))
{
VEC_COPY(contact_data->m_separating_normal,tri->m_planes.m_planes[most_edge+1]);
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
}
else
{
VEC_SCALE(contact_data->m_separating_normal,1.0f/dis,e1);
VEC_SCALE(contact_data->m_points[0],-radius,contact_data->m_separating_normal);
VEC_SUM(contact_data->m_points[0],contact_data->m_points[0],center);
}
//Scale normal for pointing to triangle
VEC_SCALE(contact_data->m_separating_normal,-1.0f,contact_data->m_separating_normal);
contact_data->m_point_count = 1;
return 1;
}
