//------------------------------------------------------------------ // Overloaded function is same as original // but in = f_in, out = f_out, true_res=0. //------------------------------------------------------------------ int DiracOpWilson::MatInv(PreserveType prs_in) { return MatInv(f_out, f_in, 0, prs_in); }
//------------------------------------------------------------------ // Overloaded function is same as original // but in = f_in and out = f_out. //------------------------------------------------------------------ int DiracOpWilson::MatInv(Float *true_res, PreserveType prs_in) { return MatInv(f_out, f_in, true_res, prs_in); }
void affTransDist(int* pMatch, int nMatch, double* pAff1, int nFeat1, double* pAff2, int nFeat2, double* pBoolSing, double* pBoolRefl, double* pDist, double* pFlip) { int i,j,idx1i,idx2i,idx1j,idx2j; double T1i[6], T2i[6], T12i[6], T21i[6], T1i_inv[6], T2i_inv[6], T_Refl[6], Temp[6]; double P1j[2], P2j[2], P1j_tran[2], P2j_tran[2]; double dist1, dist2; bool bSing, bRefl; bSing = (bool)(*pBoolSing); bRefl = (bool)(*pBoolRefl); double* pDistFlip; if(bSing) pDistFlip = new double [nMatch*nMatch]; for(i = 0; i < nMatch*nMatch; i++) pDist[i] = 0; if(bSing) for(i = 0; i < nMatch*nMatch; i++) pDistFlip[i] = 0; T_Refl[0] = 1; T_Refl[1] = 0; T_Refl[2] = 0; T_Refl[3] = 0; T_Refl[4] = -1; T_Refl[5] = 0; for(i = 0; i < nMatch; i++) { idx1i = pMatch[i]-1; idx2i = pMatch[i+nMatch]-1; T1i[0] = pAff1[idx1i]; T1i[1] = pAff1[idx1i+1*nFeat1]; T1i[2] = pAff1[idx1i+2*nFeat1]; T1i[3] = pAff1[idx1i+3*nFeat1]; T1i[4] = pAff1[idx1i+4*nFeat1]; T1i[5] = pAff1[idx1i+5*nFeat1]; T2i[0] = pAff2[idx2i]; T2i[1] = pAff2[idx2i+1*nFeat2]; T2i[2] = pAff2[idx2i+2*nFeat2]; T2i[3] = pAff2[idx2i+3*nFeat2]; T2i[4] = pAff2[idx2i+4*nFeat2]; T2i[5] = pAff2[idx2i+5*nFeat2]; for(j = 0; j < nMatch; j++) { if(i == j) continue; idx1j = pMatch[j]-1; idx2j = pMatch[j+nMatch]-1; P1j[0] = pAff1[idx1j+2*nFeat1]; P1j[1] = pAff1[idx1j+5*nFeat1]; P2j[0] = pAff2[idx2j+2*nFeat2]; P2j[1] = pAff2[idx2j+5*nFeat2]; MatInv(T1i,T1i_inv); MatInv(T2i,T2i_inv); if(bRefl) { MatMul(T2i,T_Refl,Temp,1); MatMul(Temp,T1i_inv,T12i,1); MatMul(T1i,T_Refl,Temp,1); MatMul(Temp,T2i_inv,T21i,1); } else { MatMul(T2i,T1i_inv,T12i,1); MatMul(T1i,T2i_inv,T21i,1); } MatMul(T12i,P1j,P1j_tran,2); MatMul(T21i,P2j,P2j_tran,2); dist1 = (P1j[0]-P2j_tran[0])*(P1j[0]-P2j_tran[0])+(P1j[1]-P2j_tran[1])*(P1j[1]-P2j_tran[1]); dist2 = (P2j[0]-P1j_tran[0])*(P2j[0]-P1j_tran[0])+(P2j[1]-P1j_tran[1])*(P2j[1]-P1j_tran[1]); pDist[i+nMatch*j] = (sqrt(dist1)+sqrt(dist2))/2; if(bSing) { MatMul(T21i,P1j,P1j_tran,2); MatMul(T12i,P2j,P2j_tran,2); dist1 = (P1j[0]-P2j_tran[0])*(P1j[0]-P2j_tran[0])+(P1j[1]-P2j_tran[1])*(P1j[1]-P2j_tran[1]); dist2 = (P2j[0]-P1j_tran[0])*(P2j[0]-P1j_tran[0])+(P2j[1]-P1j_tran[1])*(P2j[1]-P1j_tran[1]); pDistFlip[i+nMatch*j] = (sqrt(dist1)+sqrt(dist2))/2; } } } for(j = 0; j < nMatch-1; j++) for(i = j+1; i < nMatch; i++) { pDist[j+nMatch*i] = (pDist[j+nMatch*i]+pDist[i+nMatch*j])/2; pDist[i+nMatch*j] = pDist[j+nMatch*i]; } if(bSing) { for(j = 0; j < nMatch-1; j++) for(i = j+1; i < nMatch; i++) { pDistFlip[j+nMatch*i] = (pDistFlip[j+nMatch*i]+pDistFlip[i+nMatch*j])/2; pDistFlip[i+nMatch*j] = pDistFlip[j+nMatch*i]; } for(i = 0; i < nMatch*nMatch; i++) pFlip[i] = 0; for(i = 0; i < nMatch*nMatch; i++) if(pDist[i] > pDistFlip[i]) pFlip[i] = 1; for(i = 0; i < nMatch*nMatch; i++) if(pFlip[i]) pDist[i] = pDistFlip[i]; } if(bSing) delete [] pDistFlip; return; }
//------------------------------------------------------------------ // Overloaded function is same as original // but true_res=0. //------------------------------------------------------------------ int DiracOpWilson::MatInv(Vector *out, Vector *in, PreserveType prs_in) { return MatInv(out, in, 0, prs_in); }
//------------------------------------------------------------------ // Overloaded function is same as original // but in = f_in, out = f_out, true_res=0. //------------------------------------------------------------------ int DiracOpClover::MatInv(PreserveType prs_in) { return MatInv(f_out, f_in, 0, prs_in); }