/*!
\param triangle
\param capsule
\param contacts Contains the closest points on the capsule, and the normal points to triangle
\post The contacts array is not set to 0. It adds aditional contacts
*/
int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DATA * capsule, GDYNAMIC_ARRAY * contacts)
{
GUINT old_contact_size = contacts->m_size;
gim_closest_point_triangle_segment(triangle,capsule->m_point1,capsule->m_point2,contacts);
GIM_CONTACT * pcontact = GIM_DYNARRAY_POINTER(GIM_CONTACT ,(*contacts));
pcontact+= old_contact_size;
if(pcontact->m_depth > capsule->m_radius)
{
contacts->m_size = old_contact_size;
return 0;
}
vec3f vec;
while(old_contact_size<contacts->m_size)
{
//Scale the normal for pointing to triangle
VEC_SCALE(pcontact->m_normal,-1.0f,pcontact->m_normal);
//Fix the contact point
VEC_SCALE(vec,capsule->m_radius,pcontact->m_normal);
VEC_SUM(pcontact->m_point,vec,pcontact->m_point);
//Fix the depth
pcontact->m_depth = capsule->m_radius - pcontact->m_depth;
pcontact++;
old_contact_size++;
}
return 1;
}
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);
}
void gim_merge_contacts_unique(GDYNAMIC_ARRAY * source_contacts,
GDYNAMIC_ARRAY * dest_contacts)
{
dest_contacts->m_size = 0;
//Traverse the source contacts
GUINT32 source_count = source_contacts->m_size;
if(source_count==0) return;
GIM_CONTACT * psource_contacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,(*source_contacts));
//add the unique contact
GIM_CONTACT * pcontact = 0;
GIM_DYNARRAY_PUSH_EMPTY(GIM_CONTACT,(*dest_contacts));
pcontact = GIM_DYNARRAY_POINTER_LAST(GIM_CONTACT,(*dest_contacts));
//set the first contact
GIM_COPY_CONTACTS(pcontact, psource_contacts);
if(source_count==1) return;
//scale the first contact
VEC_SCALE(pcontact->m_normal,pcontact->m_depth,pcontact->m_normal);
psource_contacts++;
//Average the contacts
GUINT32 i;
for(i=1;i<source_count;i++)
{
VEC_SUM(pcontact->m_point,pcontact->m_point,psource_contacts->m_point);
VEC_ACCUM(pcontact->m_normal,psource_contacts->m_depth,psource_contacts->m_normal);
psource_contacts++;
}
GREAL divide_average = 1.0f/((GREAL)source_count);
VEC_SCALE(pcontact->m_point,divide_average,pcontact->m_point);
pcontact->m_depth = VEC_DOT(pcontact->m_normal,pcontact->m_normal)*divide_average;
GIM_SQRT(pcontact->m_depth,pcontact->m_depth);
VEC_NORMALIZE(pcontact->m_normal);
/*GREAL normal_len;
VEC_INV_LENGTH(pcontact->m_normal,normal_len);
VEC_SCALE(pcontact->m_normal,normal_len,pcontact->m_normal);
//Deep = LEN(normal)/SQRT(source_count)
GIM_SQRT(divide_average,divide_average);
pcontact->m_depth = divide_average/normal_len;
*/
}
static void
draw_fillets_and_join_n_norms
(int ncp,
gleDouble trimmed_loop[][3],
gleDouble untrimmed_loop[][3],
int is_trimmed[],
gleDouble bis_origin[3],
gleDouble bis_vector[3],
double normals[][3],
float front_color[3],
float back_color[3],
gleDouble cut_vector[3],
int face,
gleCapCallback cap_callback)
{
int istop;
int icnt, icnt_prev, iloop;
double *cap_loop, *norm_loop;
gleDouble sect[3];
gleDouble tmp_vec[3];
int save_style = 0;
int was_trimmed = FALSE;
save_style = gleGetJoinStyle ();
cap_loop = (double *) malloc ((ncp+3)*3*2*sizeof (double));
norm_loop = cap_loop + (ncp+3)*3;
/*
* If the first point is trimmed, keep going until one
* is found that is not trimmed, and start join there.
*/
icnt = 0;
iloop = 0;
if (!is_trimmed[0]) {
/* if the first point on the contour isn't trimmed, go ahead and
* drop an edge down to the bisecting plane, (thus starting the
* join). (Only need to do this for cut join, its bad if done for
* round join). (Also, leads to bugs when done for closed
* contours ... do this only if contour is open).
*/
if ((__TUBE_CUT_JOIN) && (!(save_style & TUBE_CONTOUR_CLOSED))) {
VEC_SUM (tmp_vec, trimmed_loop[0], bis_vector);
INNERSECT (sect,
bis_origin,
bis_vector,
trimmed_loop[0],
tmp_vec);
VEC_COPY ( (&cap_loop[3*iloop]), sect);
VEC_COPY ( (&norm_loop[3*iloop]), normals[0]);
iloop ++;
}
VEC_COPY ( (&cap_loop[3*iloop]), (trimmed_loop[0]));
VEC_COPY ( (&norm_loop[3*iloop]), normals[0]);
iloop++;
icnt_prev = icnt;
icnt ++;
} else {
/* else, loop until an untrimmed point is found */
was_trimmed = TRUE;
while (is_trimmed[icnt]) {
icnt_prev = icnt;
icnt ++;
if (icnt >= ncp) {
free (cap_loop);
return; /* oops - everything was trimmed */
}
}
}
/* Start walking around the end cap. Every time the end loop is
* trimmed, we know we'll need to draw a fillet triangle. In
* addition, after every pair of visibility changes, we draw a cap. */
if (__TUBE_CLOSE_CONTOUR) {
istop = ncp;
} else {
istop = ncp-1;
}
/* save the join style, and disable a closed contour.
* Need to do this so partial contours don't close up. */
save_style = gleGetJoinStyle ();
gleSetJoinStyle (save_style & ~TUBE_CONTOUR_CLOSED);
for (; icnt_prev < istop; icnt_prev ++, icnt ++, icnt %= ncp) {
/* There are four interesting cases for drawing caps and fillets:
* 1) this & previous point were trimmed. Don't do anything,
* advance counter.
* 2) this point trimmed, previous not -- draw fillet, and
* draw cap.
* 3) this point not trimmed, previous one was -- compute
* intersection point, draw fillet with it, and save
* point for cap contour.
* 4) this & previous point not trimmed -- save for endcap.
*/
/* Case 1 -- noop, just advance pointers */
if (is_trimmed[icnt_prev] && is_trimmed[icnt]) {
}
/* Case 2 -- Hah! first point! compute intersect & draw fillet! */
if (is_trimmed[icnt_prev] && !is_trimmed[icnt]) {
/* important note: the array "untrimmed" contains valid
* untrimmed data ONLY when is_trim is TRUE. Otherwise,
* only "trim" containes valid data */
/* compute intersection */
INNERSECT (sect,
bis_origin,
bis_vector,
untrimmed_loop[icnt_prev],
trimmed_loop[icnt]);
/* Draw Fillet */
draw_fillet_triangle_n_norms (trimmed_loop[icnt_prev],
trimmed_loop[icnt],
sect,
face,
front_color,
back_color,
normals[icnt_prev],
normals[icnt]);
VEC_COPY ( (&cap_loop[3*iloop]), sect);
VEC_COPY ( (&norm_loop[3*iloop]), normals[icnt_prev]);
iloop ++;
VEC_COPY ( (&cap_loop[3*iloop]), (trimmed_loop[icnt]));
VEC_COPY ( (&norm_loop[3*iloop]), normals[icnt]);
iloop++;
}
/* Case 3 -- add to collection of points */
if (!is_trimmed[icnt_prev] && !is_trimmed[icnt]) {
VEC_COPY ( (&cap_loop[3*iloop]), (trimmed_loop[icnt]));
VEC_COPY ( (&norm_loop[3*iloop]), normals[icnt]);
iloop++;
}
/* Case 4 -- Hah! last point! draw fillet & draw cap! */
if (!is_trimmed[icnt_prev] && is_trimmed[icnt]) {
was_trimmed = TRUE;
/* important note: the array "untrimmed" contains valid
* untrimmed data ONLY when is_trim is TRUE. Otherwise,
* only "trim" containes valid data */
/* compute intersection */
INNERSECT (sect,
bis_origin,
bis_vector,
trimmed_loop[icnt_prev],
untrimmed_loop[icnt]);
/* Draw Fillet */
draw_fillet_triangle_n_norms (trimmed_loop[icnt_prev],
trimmed_loop[icnt],
sect,
face,
front_color,
back_color,
normals[icnt_prev],
normals[icnt]);
VEC_COPY ( (&cap_loop[3*iloop]), sect);
/* OK, maybe phong normals are wrong, but at least facet
* normals will come out OK. */
if (__TUBE_DRAW_FACET_NORMALS) {
VEC_COPY ( (&norm_loop[3*iloop]), normals[icnt_prev]);
} else {
VEC_COPY ( (&norm_loop[3*iloop]), normals[icnt]);
}
iloop ++;
/* draw cap */
if (iloop >= 3) (*cap_callback) (iloop,
(gleVector *) cap_loop,
front_color,
cut_vector,
bis_vector,
(gleVector *) norm_loop,
face);
/* reset cap counter */
iloop = 0;
}
}
/* now, finish up in the same way that we started. */
icnt --; /* decrement to make up for loop exit condititons */
icnt += ncp;
icnt %= ncp;
if ((!is_trimmed[icnt]) && (iloop >= 2)) {
/* If the last point of the contour is visible, drop an edge
* to the bisecting plane, thus finishing the join.
* Note that doing this leads to bugs if done for closed
* contours ... do this only if contour is open.
*/
if ((__TUBE_CUT_JOIN) && (!(save_style & TUBE_CONTOUR_CLOSED))) {
VEC_SUM (tmp_vec, trimmed_loop[icnt], bis_vector);
INNERSECT (sect,
bis_origin,
bis_vector,
trimmed_loop[icnt],
tmp_vec);
VEC_COPY ( (&cap_loop[3*iloop]), sect);
VEC_COPY ( (&norm_loop[3*iloop]), normals[icnt]);
iloop ++;
}
/* if nothing was ever trimmed, then we want to draw the
* cap the way the user asked for it -- closed or not closed.
* Therefore, reset the closure flag to its original state.
*/
if (!was_trimmed) {
gleSetJoinStyle (save_style);
}
/* draw cap */
(*cap_callback) (iloop,
(gleVector *) cap_loop,
front_color,
cut_vector,
bis_vector,
(gleVector *) norm_loop,
face);
}
/* rest to the saved style */
gleSetJoinStyle (save_style);
free (cap_loop);
}
// capsule - trimesh By francisco leon
int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dTriMeshClass);
dIASSERT (o2->type == dCapsuleClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)o1;
dxGeom* gCylinder = o2;
//Get capsule params
dMatrix3 mCapsuleRotation;
dVector3 vCapsulePosition;
dVector3 vCapsuleAxis;
dReal vCapsuleRadius;
dReal fCapsuleSize;
dMatrix3* pRot = (dMatrix3*) dGeomGetRotation(gCylinder);
memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
dVector3* pDst = (dVector3*)dGeomGetPosition(gCylinder);
memcpy(vCapsulePosition,pDst,sizeof(dVector3));
//Axis
vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS];
vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
// Get size of CCylinder
dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
fCapsuleSize*=0.5f;
//Set Capsule params
GIM_CAPSULE_DATA capsule;
capsule.m_radius = vCapsuleRadius;
VEC_SCALE(capsule.m_point1,fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point1,vCapsulePosition,capsule.m_point1);
VEC_SCALE(capsule.m_point2,-fCapsuleSize,vCapsuleAxis);
VEC_SUM(capsule.m_point2,vCapsulePosition,capsule.m_point2);
//Create contact list
GDYNAMIC_ARRAY trimeshcontacts;
GIM_CREATE_CONTACT_LIST(trimeshcontacts);
//Collide trimeshe vs capsule
gim_trimesh_capsule_collision(&TriMesh->m_collision_trimesh,&capsule,&trimeshcontacts);
if(trimeshcontacts.m_size == 0)
{
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return 0;
}
GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
unsigned contactcount = trimeshcontacts.m_size;
unsigned contactmax = (unsigned)(flags & NUMC_MASK);
if (contactcount > contactmax)
{
contactcount = contactmax;
}
dContactGeom* pcontact;
unsigned i;
for (i=0;i<contactcount;i++)
{
pcontact = SAFECONTACT(flags, contact, i, skip);
pcontact->pos[0] = ptrimeshcontacts->m_point[0];
pcontact->pos[1] = ptrimeshcontacts->m_point[1];
pcontact->pos[2] = ptrimeshcontacts->m_point[2];
pcontact->pos[3] = 1.0f;
pcontact->normal[0] = ptrimeshcontacts->m_normal[0];
pcontact->normal[1] = ptrimeshcontacts->m_normal[1];
pcontact->normal[2] = ptrimeshcontacts->m_normal[2];
pcontact->normal[3] = 0;
pcontact->depth = ptrimeshcontacts->m_depth;
pcontact->g1 = TriMesh;
pcontact->g2 = gCylinder;
pcontact->side1 = ptrimeshcontacts->m_feature1;
pcontact->side2 = -1;
ptrimeshcontacts++;
}
GIM_DYNARRAY_DESTROY(trimeshcontacts);
return (int)contactcount;
}
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;
}
void PositLoop(int NbPts, double **centeredImage, double** homogeneousWorldPts, double**objectMat, double f,double center[2], double** RotIn, double* TransIn,double** Rot, double* Trans){
int i,j;
double deltaX, deltaY,delta=0;
double delta1,delta2;
int count=0;
bool converged= false;
double Er,Erhvmax,Er1,Erhvmax1,Er2,Erhvmax2;
long int Epr,Epr1,Epr2;
double r3T[4];
double a[3],b[3];
/* allocation for Rot1 and Rot2*/
double** Rot1;
double** Rot2;
Rot1=(double**)malloc(3*sizeof(double*));
Rot2=(double**)malloc(3*sizeof(double*));
for (i=0; i<3; i++) {
Rot1[i]=(double*)malloc(3*sizeof(double));
Rot2[i]=(double*)malloc(3*sizeof(double));
}
/* allocation for centeredImageAux*/
double** centeredImageAux;
centeredImageAux=(double **)malloc(NbPts* sizeof(double *));
for (i=0;i<NbPts;i++) centeredImageAux[i]=(double *)malloc(2 * sizeof(double));
/* end alloc*/
/* allocation for wk*/
double* wk;
wk=(double *)malloc(NbPts*sizeof(double));
/* end alloc*/
/* initializaton for Rot and Trans*/
for (i=0;i<3; i++) {
for (j=0;j<3; j++) Rot[i][j]=RotIn[i][j];
Trans[i]=TransIn[i];
}
/* initialization for imageCenteredAux*/
for (i=0;i<NbPts; i++) {
b[0]=homogeneousWorldPts[i][0];
b[1]=homogeneousWorldPts[i][1];
b[2]=homogeneousWorldPts[i][2];
MAT_DOT_VEC_3X3(a, Rot, b);
VEC_SUM(b,a,Trans);
centeredImageAux[i][0]=b[0]/b[2];
centeredImageAux[i][1]=b[1]/b[2];
}
count=0;
while (!converged) {
PositBranches(NbPts, centeredImageAux, homogeneousWorldPts, objectMat, Rot1, Rot2, Trans);
delta1=0;
delta2=0;
for (i=0; i<NbPts; i++) {
//Calculo error de proyeccion de rotacion1//
b[0]=homogeneousWorldPts[i][0];
b[1]=homogeneousWorldPts[i][1];
b[2]=homogeneousWorldPts[i][2];
MAT_DOT_VEC_3X3(a, Rot1, b);
VEC_SUM(b,a,Trans);
centeredImageAux[i][0]=b[0]/b[2];
centeredImageAux[i][1]=b[1]/b[2];
delta1+=pow((centeredImage[i][0]-centeredImageAux[i][0]),2)+pow((centeredImage[i][1]-centeredImageAux[i][1]),2);
//Calculo error de proyeccion de rotacion2//
b[0]=homogeneousWorldPts[i][0];
b[1]=homogeneousWorldPts[i][1];
b[2]=homogeneousWorldPts[i][2];
MAT_DOT_VEC_3X3(a, Rot2, b);
VEC_SUM(b,a,Trans);
centeredImageAux[i][0]=b[0]/b[2];
centeredImageAux[i][1]=b[1]/b[2];
delta2+=pow((centeredImage[i][0]-centeredImageAux[i][0]),2)+pow((centeredImage[i][1]-centeredImageAux[i][1]),2);
}
if (delta1>delta2) {
for (i=0;i<3;i++)
{
for (j=0;j<3;j++) Rot[i][j]=Rot2[i][j];
}
}
else {
for (i=0;i<3;i++)
{
for (j=0;j<3;j++) Rot[i][j]=Rot1[i][j];
}
}
// MAT_PRINT_3X3(Rot);
// VEC_PRINT(Trans);
r3T[0]=Rot[2][0];
r3T[1]=Rot[2][1];
r3T[2]=Rot[2][2];
r3T[3]=Trans[2];
/* copmute wk, and the difference between images*/
delta=0;
for (i=0;i<NbPts;i++){
wk[i]=0;
deltaX=0;
deltaY=0;
for (j=0;j<4;j++){
wk[i]+=homogeneousWorldPts[i][j]*r3T[j]/Trans[2];
}
b[0]=homogeneousWorldPts[i][0];
b[1]=homogeneousWorldPts[i][1];
b[2]=homogeneousWorldPts[i][2];
MAT_DOT_VEC_3X3(a, Rot, b);
VEC_SUM(b,a,Trans);
centeredImageAux[i][0]=b[0]/b[2];
centeredImageAux[i][1]=b[1]/b[2];
deltaX-=centeredImageAux[i][0];
deltaY-=centeredImageAux[i][1];
centeredImageAux[i][0]=wk[i]*centeredImage[i][0];
centeredImageAux[i][1]=wk[i]*centeredImage[i][1];
deltaX+=centeredImageAux[i][0];
deltaY+=centeredImageAux[i][1];
delta+=deltaX*deltaX+deltaY*deltaY;
}
// printf("puntos imagen\n");
// for (i=0; i<NbPts; i++) {
// printf("\n%f\t%f\n",centeredImageAux[i][0],centeredImageAux[i][1]);
// }
// printf("\nwk en iteracion %d\n",count);
// for (i=0; i<NbPts; i++) printf("%f\t",wk[i]);
// printf("\n");
delta=sqrt(delta);
converged=(count>0 && delta<0.001) || (count>100);
count+=1;
if (count>10&&delta>1) Rot[0][0]=2; /* if pose doesn't converge after 20 iteration, discard the pose, the value for count and delta is arbitrary*/
// printf("\nRotacion en iteracion %d: \n",count-1);
// printf("%f\t %f\t %f\n",Rot[0][0],Rot[0][1],Rot[0][2]);
// printf("%f\t %f\t %f\n",Rot[1][0],Rot[1][1],Rot[1][2]);
// printf("%f\t %f\t %f\n",Rot[2][0],Rot[2][1],Rot[2][2]);
// printf("Traslacion en iteracion %d: \n",count-1);
// printf("%f\t %f\t %f\n",Trans[0],Trans[1],Trans[2]);
}
/* deallocation*/
for (i=0;i<3;i++){
free(Rot1[i]);
free(Rot2[i]);
}
free(Rot1);
free(Rot2);
for (i=0;i<NbPts;i++){
free(centeredImageAux[i]);
}
free(centeredImageAux);
free(wk);
}
void kalman_sensors_update(kalman_state_n* state, float* measurement,float** A, float** H)
{
float **S,**Sinv;
S=(float**)malloc(6*sizeof(float*));
for (int i=0; i<6; i++) S[i]=(float *)malloc(6*sizeof(float));
Sinv=(float**)malloc(6*sizeof(float*));
for (int i=0; i<6; i++) Sinv[i]=(float *)malloc(6*sizeof(float));
float auxVec3[3],auxVec6[6],auxMat3x6[3][6],auxMat6x3[6][3],auxMat3x3[3][3];
float Atrsp[3][3];
// printf("\n \n ARRANCA KALMAN\n\n");
/*predicted p*/
TRANSPOSE_MATRIX_3X3(Atrsp, A);
MATRIX_PRODUCT_3X3(auxMat3x3, state->p, Atrsp);
MATRIX_PRODUCT_3X3(state->p, A, auxMat3x3);
ACCUM_SCALE_MATRIX_3X3(state->p, 1, state->q)
/*predicted x*/
MAT_DOT_VEC_3X3(state->x, A, state->x);
// VEC_PRINT(state->x);
/*kalman gain*/
// printf("\nARRANCA KALMAN GAIN\n");
float Htrsp[3][6];
TRANSPOSE_MATRIX_6X3(Htrsp, H);
// MAT_PRINT_3X6(Htrsp);
MATRIX_PRODUCT_3X3x3X6(auxMat3x6, state->p, Htrsp);
// MAT_PRINT_3X6(auxMat3x6);
MATRIX_PRODUCT_6X3x3X6(S, H, auxMat3x6);
ACCUM_SCALE_MATRIX_6X6(S, 1.0, state->r);
// MAT_PRINT_6X6(state->r);
// MAT_PRINT_6X6(S);
PseudoInverseGen(S, 6, 6, Sinv);
// MAT_PRINT_6X6(Sinv);
MATRIX_PRODUCT_3X6x6X6(state->k, auxMat3x6, Sinv);
// MAT_PRINT_3X6(state->k);
// printf("\TERMINA KALMAN GAIN\n");
/*estimated x*/
// VEC_PRINT(state->x);
MAT_DOT_VEC_6X3(auxVec6, H, state->x);
// VEC_PRINT_6(auxVec6);
VEC_DIFF_6(auxVec6, measurement, auxVec6);
// VEC_PRINT_6(measurement);
// VEC_PRINT_6(auxVec6);
// MAT_PRINT_3X6(state->k);
MAT_DOT_VEC_3X6(auxVec3, state->k, auxVec6);
// VEC_PRINT(auxVec3);
VEC_SUM(state->x, state->x, auxVec3);
// VEC_PRINT(state->x);
/*estimated P*/
MATRIX_PRODUCT_6X3x3X3(auxMat6x3, H, state->p);
MATRIX_PRODUCT_3X6x6X3(auxMat3x3, state->k, auxMat6x3);
ACCUM_SCALE_MATRIX_3X3(state->p, -1.0, auxMat3x3);
for (int i=0; i<6; i++) {
free(S[i]);
free(Sinv[i]);
}
free(S);
free(Sinv);
// printf("\n \n TERMINA KALMAN\n\n");
}
