void CPSRawDlg::OnSet()
{
CString str;
int usedLen;
uint16 data[MAX_HEX_DATA];
uint16 key_num = m_spin_key.GetPos();
m_ed_str.GetWindowText(str);
if (GetHexData<uint16>(str, data, MAX_HEX_DATA, &usedLen, 4))
{
if ((m_raw_len != -1) && (usedLen != m_raw_len))
{
if (MessageBox(II("The length of the data you have entered does not match the length previously read. Proceed with write?"), II("Mismatch in entry length"), MB_YESNO | MB_ICONWARNING) == IDNO)
return;
}
if (m_helper->psbc_set_pres_or_raw_if_different(key_num, data, usedLen))
{
OnRead();
}
else
{
MessageBox(II("Set failed"));
}
}
else
{
MessageBox(II("The value is not valid. Enter a hexadecimal string"));
}
m_changed = false;
}
void CPSRawDlg::OnRead()
{
uint16 data[256];
uint16 len;
uint16 key_num;
key_num = m_spin_key.GetPos();
if (m_helper->psbc_get_pres_or_raw(key_num, data, 256, &len))
{
ichar str[1024];
int i;
str[0] =II('\0');
ASSERT(len < 100);
for (i = 0; i < len; i++)
{
isprintf(str + istrlen(str), 1024 - istrlen(str), II("%04x") , data[i]);
if (i < len - 1)
isprintf(str + istrlen(str), 1024 - istrlen(str), II(" "));
}
m_ed_str.SetWindowText(str);
m_changed = false;
m_raw_len = len;
m_present = true;
}
else
{
m_present = false;
m_ed_str.SetWindowText(II("0000"));
}
m_caption_present.ShowWindow(!m_present);
}
int iisdigit(iint c)
{
#if !defined(_WINCE) && !defined(_WIN32_WCE)
return ICHAR_SELECT(isdigit, iswdigit)(c);
#else
return ((c >= II('0')) && (c <= II('9')));
#endif
}
void CPSRawDlg::OnDelete()
{
if (MessageBox(II("Are you sure you want to clear this key?"), II("Confirm delete key"), MB_ICONWARNING | MB_YESNO) == IDYES)
{
uint16 key_num = m_spin_key.GetPos();
if (!m_helper->psbc_clr(key_num))
{
MessageBox(II("Delete failed"));
}
}
}
/* ionosphere residuals ------------------------------------------------------*/
static int res_iono(const obsd_t *obs, int n, const nav_t *nav,
const double *rs, const double *rr, const double *pos,
const double *azel, const pcv_t *pcv, const ekf_t *ekf,
double *phw, double *v, double *H, double *R)
{
double *sig,P1,P2,L1,L2,c_iono=1.0-SQR(lam[1]/lam[0]);
double LG,PG,antdel[3]={0},dant[NFREQ]={0};
int i,j,nv=0,sat;
sig=mat(1,2*n);
for (i=0;i<n;i++) {
if (!raw_obs(obs+i,nav,&P1,&P2,&L1,&L2)||azel[i*2+1]<MIN_EL) continue;
sat=obs[i].sat;
/* ionosphere-LC model */
LG=-c_iono*ekf->x[II(sat)]+ekf->x[IB(sat)];
PG= c_iono*ekf->x[II(sat)]+nav->cbias[sat-1][0];
/* receiver antenna phase center offset and variation */
if (pcv) {
antmodel(pcv,antdel,azel+i*2,dant);
LG+=dant[0]-dant[1];
PG+=dant[0]-dant[1];
}
/* phase windup correction */
windupcorr(obs[i].time,rs+i*6,rr,phw+obs[i].sat-1);
LG+=(lam[0]-lam[1])*phw[obs[i].sat-1];
/* residuals of ionosphere (geometriy-free) LC */
v[nv ]=(L1-L2)-LG;
#if 0
v[nv+1]=(P1-P2)-PG;
#else
v[nv+1]=0.0;
#endif
for (j=0;j<ekf->nx*2;j++) H[ekf->nx*nv+j]=0.0;
H[ekf->nx*nv +II(sat)]=-c_iono;
H[ekf->nx*nv +IB(sat)]=1.0;
H[ekf->nx*(nv+1)+II(sat)]=c_iono;
sig[nv ]=sig_err(azel+i*2);
sig[nv+1]=RATIO_ERR*sig[nv];
nv+=2;
}
for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
R[i+j*nv]=i==j?SQR(sig[i]):0.0;
}
free(sig);
return nv;
}
/* temporal update of states --------------------------------------------------*/
static void ud_state(const obsd_t *obs, int n, const nav_t *nav,
const double *pos, const double *azel, ekf_t *ekf,
sstat_t *sstat)
{
double P1,P2,L1,L2,PG,LG,tt,F[4]={0},Q[2]={0};
double x[2]={0},P[2],c_iono=1.0-SQR(lam[1]/lam[0]);
int i,sat,slip;
for (i=0;i<n;i++) {
/* raw pseudorange and phase range */
if (!raw_obs(obs+i,nav,&P1,&P2,&L1,&L2)||azel[i*2+1]<MIN_EL) continue;
sat=obs[i].sat;
tt=timediff(obs[i].time,sstat[sat-1].time);
LG=L1-L2;
PG=P1-P2;
slip=(obs[i].LLI[0]&3)||(obs[i].LLI[1]&3);
slip|=fabs(LG-sstat[sat-1].LG)>THRES_LG;
if (fabs(tt)>MAXGAP_IONO) {
#if 1
x[0]=PG/c_iono;
#else
x[0]=ionmodel(obs[i].time,nav->ion_gps,pos,azel+i*2);
#endif
x[1]=1E-6;
P[0]=VAR_IONO;
P[1]=VAR_IONR;
ekf_init(ekf,x,P,II(sat),2);
}
else {
F[0]=F[3]=1.0;
F[2]=tt;
Q[0]=PRN_IONO*fabs(tt);
Q[1]=PRN_IONR*fabs(tt);
ekf_pred(ekf,F,Q,II(sat),2);
}
if (tt>MAXGAP_BIAS||slip) {
x[0]=LG+PG;
P[0]=VAR_BIAS;
ekf_init(ekf,x,P,IB(sat),1);
}
sstat[sat-1].time=obs[i].time;
sstat[sat-1].azel[0]=azel[i*2];
sstat[sat-1].azel[1]=azel[i*2+1];
sstat[sat-1].slip=slip;
sstat[sat-1].LG=LG;
sstat[sat-1].PG=PG;
}
}
static bool f14h_mc0_mce(u16 ec, u8 xec)
{
u8 r4 = R4(ec);
bool ret = true;
if (MEM_ERROR(ec)) {
if (TT(ec) != TT_DATA || LL(ec) != LL_L1)
return false;
switch (r4) {
case R4_DRD:
case R4_DWR:
pr_cont("Data/Tag parity error due to %s.\n",
(r4 == R4_DRD ? "load/hw prf" : "store"));
break;
case R4_EVICT:
pr_cont("Copyback parity error on a tag miss.\n");
break;
case R4_SNOOP:
pr_cont("Tag parity error during snoop.\n");
break;
default:
ret = false;
}
} else if (BUS_ERROR(ec)) {
if ((II(ec) != II_MEM && II(ec) != II_IO) || LL(ec) != LL_LG)
return false;
pr_cont("System read data error on a ");
switch (r4) {
case R4_RD:
pr_cont("TLB reload.\n");
break;
case R4_DWR:
pr_cont("store.\n");
break;
case R4_DRD:
pr_cont("load.\n");
break;
default:
ret = false;
}
} else {
ret = false;
}
return ret;
}
/* temporal update of states --------------------------------------------------*/
static void udstate(const obsd_t *obs, int n, const nav_t *nav, double *x,
double *P, int nx, ssat_t *ssat)
{
gtime_t time;
double tt;
int i,sat;
for (i=0;i<n;i++) {
sat=obs[i].sat
time=ssat[sat-1].time;
if (!time.time) {
init_iono(obs+i,nav,x,P,nx);
init_bias(obs+i,nav,x,P,nx);
}
else {
tt=timediff(obs[i].time,time);
P[II(sat)*(nx+1)]+=PRN_IONO*fabs(tt);
if (det_slip(obs+i,nav,ssat)||fabs(tt)>MAXGAP_BIAS) {
init_bias(obs+i,nav,x,P,nx);
}
}
ssat[sat-1].time=time;
}
}
BOOL CSmallPolygonsVisDlg::OnInitDialog()
{
CDialogEx::OnInitDialog();
// Set the icon for this dialog. The framework does this automatically
// when the application's main window is not a dialog
SetIcon(m_hIcon, TRUE); // Set big icon
SetIcon(m_hIcon, FALSE); // Set small icon
IISet s;
for (int i = 0; i < 100; ++i) {
int x = (rand() * MAX_COORDINATE) / (RAND_MAX + 1);
int y = (rand() * MAX_COORDINATE) / (RAND_MAX + 1);
s.insert(II(x, y));
}
int N = 10;
int Np = s.size();
std::vector<int> points;
for (auto p : s) {
points.push_back(p.first);
points.push_back(p.second);
}
m_sp.init(points, N);
m_sp.findTriangles();
m_sp.chooseFirstPolygon();
for (int i = 0; i < 100; ++i) {
m_sp.chooseNextVertex();
}
return TRUE; // return TRUE unless you set the focus to a control
}
void process(int vtx){
taken[vtx] = 1;
for(int j=0; j<(int)adjList[vtx].size(); j++){
II v = adjList[vtx][j];
if(!taken[v.first]) pq.push(II(-v.second,-v.first));
}
}
void recorreIzqDerAVL( AVL *a, AVL *aux, int *band )
{
if( !(*a) -> der )
{
*aux = *a;
*a = (*a) -> izq;
}else
{
recorreIzqDerAVL( &(*a) -> der, aux, band );
if( *band )
{
if( (*a) -> fe == 0 )
{
(*a) -> fe == -1;
*band = 0;
}else
{
if( (*a) -> fe == 1 )
(*a) -> fe = 0;
else
if( (*a) -> izq -> fe <= 0 )
{
if( (*a) -> izq -> fe == 0 )
*band = 0;
II( a );
}else
ID( a );
}
}
}
}
int buscarDerechaAVL( AVL *a, int dato, int *band )
{
int retAux;
retAux = eliminaAVL( &(*a)->der, dato, band );
if( *band )
{
if( (*a) -> fe == 0 )
{
(*a) -> fe = -1;
*band = 0;
}else
if( (*a) -> fe == 1 )
(*a) -> fe = 0;
else
{
if( (*a) -> der -> fe <= 0 )
{
if( (*a) -> der -> fe == 0 )
*band = 0;
II( a );
}else
ID( a );
}
}
return( retAux );
}
bool mrpt::vision::pnp::ppnp::compute_pose(
Eigen::Matrix3d& R, Eigen::Vector3d& t, int n)
{
double tol = 0.00001;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n, n), A(n, n), Y(n, 3),
E(n, 3), E_old(n, 3), U, V,
I3 = Eigen::MatrixXd::Identity(3, 3), PR,
Z = Eigen::MatrixXd::Zero(n, n);
Eigen::VectorXd e(n), II(n), c(3), Zmindiag(n);
e.fill(1);
II.fill(1.0 / ((double)n));
A = I - e * e.transpose() / n;
double err = std::numeric_limits<double>::infinity();
E_old.fill(1000);
int cnt = 0;
while (err > tol)
{
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
P.transpose() * Z * A * S,
Eigen::ComputeThinU | Eigen::ComputeThinV);
U = svd.matrixU();
V = svd.matrixV();
I3(2, 2) = (U * V.transpose()).determinant();
R = U * I3 * V.transpose();
PR = P * R;
c = (S - Z * PR).transpose() * II;
Y = S - e * c.transpose();
Zmindiag = ((PR * Y.transpose()).diagonal()).array() /
((P.array() * P.array()).rowwise().sum()).array();
for (int i = 0; i < n; i++)
if (Zmindiag(i) < 0) Zmindiag(i) = 0;
Z = Zmindiag.asDiagonal();
E = Y - Z * PR;
err = (E - E_old).norm();
E_old = E;
cnt++;
}
t = -R * c;
return true;
}
long istrtol(const ichar *nptr, ichar **endptr, int base)
{
#if !defined _WINCE && !defined _WIN32_WCE
return ICHAR_SELECT(strtol, wcstol)(nptr, endptr, base);
#else
// ASSERT(base == 10);
const ichar *p = nptr;
if (*p == II('-'))
{
p++;
return 0L - (long)istrtoul(p, endptr, base);
}
if (*p == II('+'))
p++;
return (long) istrtoul(p, endptr, base);
#endif
}
void BMatSetZero(BMat& C, BMat& mat) {
for(int I = 0; I < (int)C.size(); I++) {
pair<Irrep, Irrep> II(I, I);
MatrixXcd& CII = C[II];
mat[II] = MatrixXcd::Zero(CII.rows(), CII.cols());
}
}
/* initizlize ionosphere parameter --------------------------------------------*/
static void init_iono(const obsd_t *obs, const double *azel, double *x,
double *P, int nx)
{
double map,iono;
if (obs->P[0]==0||obs->P[1]==0) return;
map=ionmapf(pos,azel);
iono=(obs->P[0]-obs->P[1])/map;
initx(x,P,nx,II(obs->sat),iono,VAR_IONO);
}
void dijkstra(int src, IV &dis) {
set<II> q;
dis = IV(n, INF);
BV vis(n);
q.insert(II(0, src));
dis[src] = 0;
while (! q.empty()) {
II p = *(q.begin()); q.erase(q.begin());
int d = p.first, v = p.second;
if (vis[v]) continue;
vis[v] = true;
For (EL, e, adj[v]) {
int d2 = d + e->w;
if (d2 < dis[e->v]) {
dis[e->v] = d2;
q.insert(II(d2, e->v));
}
}
}
/* output ionosphere parameters ----------------------------------------------*/
static void out_iono(gtime_t time, const ekf_t *ekf, const sstat_t *sstat,
FILE *fp)
{
double tow;
char id[64];
int sat,week;
tow=time2gpst(time,&week);
for (sat=1;sat<=MAXSAT;sat++) {
if (sstat[sat-1].time.time==0||
timediff(time,sstat[sat-1].time)>MAXGAP_IONO) continue;
satno2id(sat,id);
fprintf(fp,"%s %4d %6.0f %-3s %d %8.4f %9.6f %7.4f %6.1f %5.1f %7.3f %11.3f\n",
STEC_RID,week,tow,id,sstat[sat-1].slip,
ekf->x[II(sat)],ekf->x[II(sat)+1],
sqrt(ekf->P[II(sat)*(ekf->nx+1)]),sstat[sat-1].azel[0]*R2D,
sstat[sat-1].azel[1]*R2D, sstat[sat-1].PG,sstat[sat-1].LG);
}
}
int insAVL( AVL *a, int dato, int *band )
{
int ret;
if( !*a )
ret = *band = creaNodoAVL( a , dato );
else
{
if( dato < (*a) -> info )
{
ret = insAVL( &(*a) -> izq, dato, band );
if( *band )
if( (*a) -> fe == 0 )
(*a) -> fe = -1;
else
if( (*a ) -> fe == 1 )
*band = (*a) -> fe = 0;
else
{
if( (*a) -> izq -> fe > 0 )
ID( a );
else
II( a );
*band = 0;
}
}else
if( dato > (*a) -> info )
{
ret = insAVL( &(*a) -> der, dato, band );
if( *band )
if( (*a) -> fe == 0 )
(*a) -> fe = 1;
else
if( (*a ) -> fe == -1 )
*band = (*a) -> fe = 0;
else
{
if( (*a) -> der -> fe >= 0 )
DD( a );
else
DI( a );
*band = 0;
}
}else
ret = *band = 0;
}
return( ret );
}
void dgDynamicBodyAsymetric::SetMassMatrix(dgFloat32 mass, const dgMatrix& inertia)
{
//dgVector II;
m_principalAxis = inertia;
dgVector II (m_principalAxis.EigenVectors());
dgMatrix massMatrix(dgGetIdentityMatrix());
massMatrix[0][0] = II[0];
massMatrix[1][1] = II[1];
massMatrix[2][2] = II[2];
dgBody::SetMassMatrix(mass, massMatrix);
}
void calcRanks() {
II list[MAX_STUDENTS];
for ( int i = 0; i < si_cnt; ++ i )
list[i] = II(si_list[i].getSum(), i);
sort(list, list + si_cnt, greater<II>());
for ( int i = 0; i < si_cnt; ++ i ) {
if ( i > 0 && list[i].first == list[i - 1].first )
ranks[list[i].second] = ranks[list[i - 1].second];
else
ranks[list[i].second] = i + 1;
}
}
/* ionosphere residuals ------------------------------------------------------*/
static int res_iono(const obsd_t *obs, int n, const double *azel,
const double *x, int nx, double *v, double *H, double *R)
{
double *sig,L1,L2,P1,P2,map;
int i,j,nv=0,sat;
sig=mat(1,2*n);
for (i=0;i<n;i++) {
sat=obs[i].sat;
L1=obs->L[0]*lam[0];
L2=obs->L[1]*lam[1];
P1=obs->P[0];
P2=obs->P[1];
if (L1==0||L2==0||P1==0||P2==0) continue;
/* ionosphere mapping function */
map=ionmapf(pos,azel+i*2);
/* residuals of ionosphere (geometriy-free) LC */
v[nv ]=(L1-L2)+map*x[II(sat)]-x[IB(sat)];
v[nv+1]=(P1-P2)-map*x[II(sat)];
/* partial derivatives */
for (j=0;j<nx;j++) H[nx*nv+j]=0.0;
H[nx*nv +II(sat)]=-map;
H[nx*nv +IB(sat)]=1.0;
H[nx*(nv+1)+IB(sat)]=map;
/* standard deviation of error */
sig[nv ]=std_err(azel);
sig[nv+1]=sig[nv]*RATIO_ERR;
nv+=2;
}
for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
R[i+j*nv]=i==j?SQR(sig[i]):0.0;
}
free(sig);
return nv;
}
int prim_mst(int src) {
IIS q;
IV dis(n, INF);
BV flg(n);
dis[src] = 0;
q.insert(II(0, src));
int mst = 0;
while (! q.empty()) {
int d = q.begin()->first;
int v = q.begin()->second;
q.erase(q.begin());
if (flg[v]) continue;
flg[v] = true;
mst += d;
For (EL, e, adj[v])
if (!flg[e->v] && e->w < dis[e->v]) {
dis[e->v] = e->w;
q.insert(II(dis[e->v], e->v));
}
}
return mst;
}
void DecodeCallBack::onTransportFailure (FailureMode f)
{
{
#ifdef THREADING_WORKAROUND
CriticalSection::Lock cs_lock (cs);
#endif
printTimestamp ();
iprintlf(II("*** HCI TRANSPORT FAILED *** (failureMode: "));
printByValue(transportErrors_d, f, 16, "Unknown transport error");
iprintlf(II(")"));
#ifdef DONT_COMPLETELY_SCREW_UP_ANYONE_EXPECTING_NORMAL_END_OF_LINE_TERMINATORS
printlf("\n");
#else
printf ("\r"); printlfOnly ("\n");
fflush (stdout);
}
if (auto_rawlog)
{
parseCmd("rawlog", true);
}
#endif
}
/* output ionosphere parameters ----------------------------------------------*/
static int out_iono(gtime_t time, const double *x, const double *P, int nx,
FILE *fp)
{
double tow;
char id[64];
int i,week;
tow=time2gpst(time,&week);
for (i=0;i<MAXSAT;i++) {
sat2id(i+1,id);
fprintf(fp,"$ION,%d,%.3f,%d,%s,%.1f,%.4f,%4f\n",week,tow,0,id,
ssat[i].azel[1]*R2D,x[II(i+1)],0);
}
}
unsigned long istrtoul(const ichar *nptr, ichar **endptr, int base)
{
#if !defined _WINCE && !defined _WIN32_WCE
return ICHAR_SELECT(strtoul, wcstoul)(nptr, endptr, base);
#else
const ichar *p = nptr;
unsigned long val = 0;
// ASSERT(base == 10);
while (iisdigit(*p))
{
val = val * 10;
val = val + ((*p) - II('0'));
p++;
}
if (endptr)
*endptr = (ichar *)p;
return val;
#endif
}
/*
* MD5 Compression Function
*/
void MD5::compress_n(const byte input[], size_t blocks)
{
u32bit A = digest[0], B = digest[1], C = digest[2], D = digest[3];
for(size_t i = 0; i != blocks; ++i)
{
load_le(&M[0], input, M.size());
FF(A,B,C,D,M[ 0], 7,0xD76AA478); FF(D,A,B,C,M[ 1],12,0xE8C7B756);
FF(C,D,A,B,M[ 2],17,0x242070DB); FF(B,C,D,A,M[ 3],22,0xC1BDCEEE);
FF(A,B,C,D,M[ 4], 7,0xF57C0FAF); FF(D,A,B,C,M[ 5],12,0x4787C62A);
FF(C,D,A,B,M[ 6],17,0xA8304613); FF(B,C,D,A,M[ 7],22,0xFD469501);
FF(A,B,C,D,M[ 8], 7,0x698098D8); FF(D,A,B,C,M[ 9],12,0x8B44F7AF);
FF(C,D,A,B,M[10],17,0xFFFF5BB1); FF(B,C,D,A,M[11],22,0x895CD7BE);
FF(A,B,C,D,M[12], 7,0x6B901122); FF(D,A,B,C,M[13],12,0xFD987193);
FF(C,D,A,B,M[14],17,0xA679438E); FF(B,C,D,A,M[15],22,0x49B40821);
GG(A,B,C,D,M[ 1], 5,0xF61E2562); GG(D,A,B,C,M[ 6], 9,0xC040B340);
GG(C,D,A,B,M[11],14,0x265E5A51); GG(B,C,D,A,M[ 0],20,0xE9B6C7AA);
GG(A,B,C,D,M[ 5], 5,0xD62F105D); GG(D,A,B,C,M[10], 9,0x02441453);
GG(C,D,A,B,M[15],14,0xD8A1E681); GG(B,C,D,A,M[ 4],20,0xE7D3FBC8);
GG(A,B,C,D,M[ 9], 5,0x21E1CDE6); GG(D,A,B,C,M[14], 9,0xC33707D6);
GG(C,D,A,B,M[ 3],14,0xF4D50D87); GG(B,C,D,A,M[ 8],20,0x455A14ED);
GG(A,B,C,D,M[13], 5,0xA9E3E905); GG(D,A,B,C,M[ 2], 9,0xFCEFA3F8);
GG(C,D,A,B,M[ 7],14,0x676F02D9); GG(B,C,D,A,M[12],20,0x8D2A4C8A);
HH(A,B,C,D,M[ 5], 4,0xFFFA3942); HH(D,A,B,C,M[ 8],11,0x8771F681);
HH(C,D,A,B,M[11],16,0x6D9D6122); HH(B,C,D,A,M[14],23,0xFDE5380C);
HH(A,B,C,D,M[ 1], 4,0xA4BEEA44); HH(D,A,B,C,M[ 4],11,0x4BDECFA9);
HH(C,D,A,B,M[ 7],16,0xF6BB4B60); HH(B,C,D,A,M[10],23,0xBEBFBC70);
HH(A,B,C,D,M[13], 4,0x289B7EC6); HH(D,A,B,C,M[ 0],11,0xEAA127FA);
HH(C,D,A,B,M[ 3],16,0xD4EF3085); HH(B,C,D,A,M[ 6],23,0x04881D05);
HH(A,B,C,D,M[ 9], 4,0xD9D4D039); HH(D,A,B,C,M[12],11,0xE6DB99E5);
HH(C,D,A,B,M[15],16,0x1FA27CF8); HH(B,C,D,A,M[ 2],23,0xC4AC5665);
II(A,B,C,D,M[ 0], 6,0xF4292244); II(D,A,B,C,M[ 7],10,0x432AFF97);
II(C,D,A,B,M[14],15,0xAB9423A7); II(B,C,D,A,M[ 5],21,0xFC93A039);
II(A,B,C,D,M[12], 6,0x655B59C3); II(D,A,B,C,M[ 3],10,0x8F0CCC92);
II(C,D,A,B,M[10],15,0xFFEFF47D); II(B,C,D,A,M[ 1],21,0x85845DD1);
II(A,B,C,D,M[ 8], 6,0x6FA87E4F); II(D,A,B,C,M[15],10,0xFE2CE6E0);
II(C,D,A,B,M[ 6],15,0xA3014314); II(B,C,D,A,M[13],21,0x4E0811A1);
II(A,B,C,D,M[ 4], 6,0xF7537E82); II(D,A,B,C,M[11],10,0xBD3AF235);
II(C,D,A,B,M[ 2],15,0x2AD7D2BB); II(B,C,D,A,M[ 9],21,0xEB86D391);
A = (digest[0] += A);
B = (digest[1] += B);
C = (digest[2] += C);
D = (digest[3] += D);
input += hash_block_size();
}
}
void MD5::Transform(const uint8_t Block[64], int& error)
{
uint32_t a = m_lMD5[0];
uint32_t b = m_lMD5[1];
uint32_t c = m_lMD5[2];
uint32_t d = m_lMD5[3];
uint32_t X[16];
ByteToUINT( X, Block, 64, error);
//Round 1 Transformation
FF (a, b, c, d, X[ 0], MD5_S11, MD5_T01);
FF (d, a, b, c, X[ 1], MD5_S12, MD5_T02);
FF (c, d, a, b, X[ 2], MD5_S13, MD5_T03);
FF (b, c, d, a, X[ 3], MD5_S14, MD5_T04);
FF (a, b, c, d, X[ 4], MD5_S11, MD5_T05);
FF (d, a, b, c, X[ 5], MD5_S12, MD5_T06);
FF (c, d, a, b, X[ 6], MD5_S13, MD5_T07);
FF (b, c, d, a, X[ 7], MD5_S14, MD5_T08);
FF (a, b, c, d, X[ 8], MD5_S11, MD5_T09);
FF (d, a, b, c, X[ 9], MD5_S12, MD5_T10);
FF (c, d, a, b, X[10], MD5_S13, MD5_T11);
FF (b, c, d, a, X[11], MD5_S14, MD5_T12);
FF (a, b, c, d, X[12], MD5_S11, MD5_T13);
FF (d, a, b, c, X[13], MD5_S12, MD5_T14);
FF (c, d, a, b, X[14], MD5_S13, MD5_T15);
FF (b, c, d, a, X[15], MD5_S14, MD5_T16);
//Round 2 Transformation
GG (a, b, c, d, X[ 1], MD5_S21, MD5_T17);
GG (d, a, b, c, X[ 6], MD5_S22, MD5_T18);
GG (c, d, a, b, X[11], MD5_S23, MD5_T19);
GG (b, c, d, a, X[ 0], MD5_S24, MD5_T20);
GG (a, b, c, d, X[ 5], MD5_S21, MD5_T21);
GG (d, a, b, c, X[10], MD5_S22, MD5_T22);
GG (c, d, a, b, X[15], MD5_S23, MD5_T23);
GG (b, c, d, a, X[ 4], MD5_S24, MD5_T24);
GG (a, b, c, d, X[ 9], MD5_S21, MD5_T25);
GG (d, a, b, c, X[14], MD5_S22, MD5_T26);
GG (c, d, a, b, X[ 3], MD5_S23, MD5_T27);
GG (b, c, d, a, X[ 8], MD5_S24, MD5_T28);
GG (a, b, c, d, X[13], MD5_S21, MD5_T29);
GG (d, a, b, c, X[ 2], MD5_S22, MD5_T30);
GG (c, d, a, b, X[ 7], MD5_S23, MD5_T31);
GG (b, c, d, a, X[12], MD5_S24, MD5_T32);
//Round 3 Transformation
HH (a, b, c, d, X[ 5], MD5_S31, MD5_T33);
HH (d, a, b, c, X[ 8], MD5_S32, MD5_T34);
HH (c, d, a, b, X[11], MD5_S33, MD5_T35);
HH (b, c, d, a, X[14], MD5_S34, MD5_T36);
HH (a, b, c, d, X[ 1], MD5_S31, MD5_T37);
HH (d, a, b, c, X[ 4], MD5_S32, MD5_T38);
HH (c, d, a, b, X[ 7], MD5_S33, MD5_T39);
HH (b, c, d, a, X[10], MD5_S34, MD5_T40);
HH (a, b, c, d, X[13], MD5_S31, MD5_T41);
HH (d, a, b, c, X[ 0], MD5_S32, MD5_T42);
HH (c, d, a, b, X[ 3], MD5_S33, MD5_T43);
HH (b, c, d, a, X[ 6], MD5_S34, MD5_T44);
HH (a, b, c, d, X[ 9], MD5_S31, MD5_T45);
HH (d, a, b, c, X[12], MD5_S32, MD5_T46);
HH (c, d, a, b, X[15], MD5_S33, MD5_T47);
HH (b, c, d, a, X[ 2], MD5_S34, MD5_T48);
//Round 4 Transformation
II (a, b, c, d, X[ 0], MD5_S41, MD5_T49);
II (d, a, b, c, X[ 7], MD5_S42, MD5_T50);
II (c, d, a, b, X[14], MD5_S43, MD5_T51);
II (b, c, d, a, X[ 5], MD5_S44, MD5_T52);
II (a, b, c, d, X[12], MD5_S41, MD5_T53);
II (d, a, b, c, X[ 3], MD5_S42, MD5_T54);
II (c, d, a, b, X[10], MD5_S43, MD5_T55);
II (b, c, d, a, X[ 1], MD5_S44, MD5_T56);
II (a, b, c, d, X[ 8], MD5_S41, MD5_T57);
II (d, a, b, c, X[15], MD5_S42, MD5_T58);
II (c, d, a, b, X[ 6], MD5_S43, MD5_T59);
II (b, c, d, a, X[13], MD5_S44, MD5_T60);
II (a, b, c, d, X[ 4], MD5_S41, MD5_T61);
II (d, a, b, c, X[11], MD5_S42, MD5_T62);
II (c, d, a, b, X[ 2], MD5_S43, MD5_T63);
II (b, c, d, a, X[ 9], MD5_S44, MD5_T64);
m_lMD5[0] += a;
m_lMD5[1] += b;
m_lMD5[2] += c;
m_lMD5[3] += d;
}
static int rmd128_compress(hash_state *md, unsigned char *buf)
#endif
{
ulong32 aa,bb,cc,dd,aaa,bbb,ccc,ddd,X[16];
int i;
/* load words X */
for (i = 0; i < 16; i++){
LOAD32L(X[i], buf + (4 * i));
}
/* load state */
aa = aaa = md->rmd128.state[0];
bb = bbb = md->rmd128.state[1];
cc = ccc = md->rmd128.state[2];
dd = ddd = md->rmd128.state[3];
/* round 1 */
FF(aa, bb, cc, dd, X[ 0], 11);
FF(dd, aa, bb, cc, X[ 1], 14);
FF(cc, dd, aa, bb, X[ 2], 15);
FF(bb, cc, dd, aa, X[ 3], 12);
FF(aa, bb, cc, dd, X[ 4], 5);
FF(dd, aa, bb, cc, X[ 5], 8);
FF(cc, dd, aa, bb, X[ 6], 7);
FF(bb, cc, dd, aa, X[ 7], 9);
FF(aa, bb, cc, dd, X[ 8], 11);
FF(dd, aa, bb, cc, X[ 9], 13);
FF(cc, dd, aa, bb, X[10], 14);
FF(bb, cc, dd, aa, X[11], 15);
FF(aa, bb, cc, dd, X[12], 6);
FF(dd, aa, bb, cc, X[13], 7);
FF(cc, dd, aa, bb, X[14], 9);
FF(bb, cc, dd, aa, X[15], 8);
/* round 2 */
GG(aa, bb, cc, dd, X[ 7], 7);
GG(dd, aa, bb, cc, X[ 4], 6);
GG(cc, dd, aa, bb, X[13], 8);
GG(bb, cc, dd, aa, X[ 1], 13);
GG(aa, bb, cc, dd, X[10], 11);
GG(dd, aa, bb, cc, X[ 6], 9);
GG(cc, dd, aa, bb, X[15], 7);
GG(bb, cc, dd, aa, X[ 3], 15);
GG(aa, bb, cc, dd, X[12], 7);
GG(dd, aa, bb, cc, X[ 0], 12);
GG(cc, dd, aa, bb, X[ 9], 15);
GG(bb, cc, dd, aa, X[ 5], 9);
GG(aa, bb, cc, dd, X[ 2], 11);
GG(dd, aa, bb, cc, X[14], 7);
GG(cc, dd, aa, bb, X[11], 13);
GG(bb, cc, dd, aa, X[ 8], 12);
/* round 3 */
HH(aa, bb, cc, dd, X[ 3], 11);
HH(dd, aa, bb, cc, X[10], 13);
HH(cc, dd, aa, bb, X[14], 6);
HH(bb, cc, dd, aa, X[ 4], 7);
HH(aa, bb, cc, dd, X[ 9], 14);
HH(dd, aa, bb, cc, X[15], 9);
HH(cc, dd, aa, bb, X[ 8], 13);
HH(bb, cc, dd, aa, X[ 1], 15);
HH(aa, bb, cc, dd, X[ 2], 14);
HH(dd, aa, bb, cc, X[ 7], 8);
HH(cc, dd, aa, bb, X[ 0], 13);
HH(bb, cc, dd, aa, X[ 6], 6);
HH(aa, bb, cc, dd, X[13], 5);
HH(dd, aa, bb, cc, X[11], 12);
HH(cc, dd, aa, bb, X[ 5], 7);
HH(bb, cc, dd, aa, X[12], 5);
/* round 4 */
II(aa, bb, cc, dd, X[ 1], 11);
II(dd, aa, bb, cc, X[ 9], 12);
II(cc, dd, aa, bb, X[11], 14);
II(bb, cc, dd, aa, X[10], 15);
II(aa, bb, cc, dd, X[ 0], 14);
II(dd, aa, bb, cc, X[ 8], 15);
II(cc, dd, aa, bb, X[12], 9);
II(bb, cc, dd, aa, X[ 4], 8);
II(aa, bb, cc, dd, X[13], 9);
II(dd, aa, bb, cc, X[ 3], 14);
II(cc, dd, aa, bb, X[ 7], 5);
II(bb, cc, dd, aa, X[15], 6);
II(aa, bb, cc, dd, X[14], 8);
II(dd, aa, bb, cc, X[ 5], 6);
II(cc, dd, aa, bb, X[ 6], 5);
II(bb, cc, dd, aa, X[ 2], 12);
/* parallel round 1 */
III(aaa, bbb, ccc, ddd, X[ 5], 8);
III(ddd, aaa, bbb, ccc, X[14], 9);
III(ccc, ddd, aaa, bbb, X[ 7], 9);
III(bbb, ccc, ddd, aaa, X[ 0], 11);
III(aaa, bbb, ccc, ddd, X[ 9], 13);
III(ddd, aaa, bbb, ccc, X[ 2], 15);
III(ccc, ddd, aaa, bbb, X[11], 15);
III(bbb, ccc, ddd, aaa, X[ 4], 5);
III(aaa, bbb, ccc, ddd, X[13], 7);
III(ddd, aaa, bbb, ccc, X[ 6], 7);
III(ccc, ddd, aaa, bbb, X[15], 8);
III(bbb, ccc, ddd, aaa, X[ 8], 11);
III(aaa, bbb, ccc, ddd, X[ 1], 14);
III(ddd, aaa, bbb, ccc, X[10], 14);
III(ccc, ddd, aaa, bbb, X[ 3], 12);
III(bbb, ccc, ddd, aaa, X[12], 6);
/* parallel round 2 */
HHH(aaa, bbb, ccc, ddd, X[ 6], 9);
HHH(ddd, aaa, bbb, ccc, X[11], 13);
HHH(ccc, ddd, aaa, bbb, X[ 3], 15);
HHH(bbb, ccc, ddd, aaa, X[ 7], 7);
HHH(aaa, bbb, ccc, ddd, X[ 0], 12);
HHH(ddd, aaa, bbb, ccc, X[13], 8);
HHH(ccc, ddd, aaa, bbb, X[ 5], 9);
HHH(bbb, ccc, ddd, aaa, X[10], 11);
HHH(aaa, bbb, ccc, ddd, X[14], 7);
HHH(ddd, aaa, bbb, ccc, X[15], 7);
HHH(ccc, ddd, aaa, bbb, X[ 8], 12);
HHH(bbb, ccc, ddd, aaa, X[12], 7);
HHH(aaa, bbb, ccc, ddd, X[ 4], 6);
HHH(ddd, aaa, bbb, ccc, X[ 9], 15);
HHH(ccc, ddd, aaa, bbb, X[ 1], 13);
HHH(bbb, ccc, ddd, aaa, X[ 2], 11);
/* parallel round 3 */
GGG(aaa, bbb, ccc, ddd, X[15], 9);
GGG(ddd, aaa, bbb, ccc, X[ 5], 7);
GGG(ccc, ddd, aaa, bbb, X[ 1], 15);
GGG(bbb, ccc, ddd, aaa, X[ 3], 11);
GGG(aaa, bbb, ccc, ddd, X[ 7], 8);
GGG(ddd, aaa, bbb, ccc, X[14], 6);
GGG(ccc, ddd, aaa, bbb, X[ 6], 6);
GGG(bbb, ccc, ddd, aaa, X[ 9], 14);
GGG(aaa, bbb, ccc, ddd, X[11], 12);
GGG(ddd, aaa, bbb, ccc, X[ 8], 13);
GGG(ccc, ddd, aaa, bbb, X[12], 5);
GGG(bbb, ccc, ddd, aaa, X[ 2], 14);
GGG(aaa, bbb, ccc, ddd, X[10], 13);
GGG(ddd, aaa, bbb, ccc, X[ 0], 13);
GGG(ccc, ddd, aaa, bbb, X[ 4], 7);
GGG(bbb, ccc, ddd, aaa, X[13], 5);
/* parallel round 4 */
FFF(aaa, bbb, ccc, ddd, X[ 8], 15);
FFF(ddd, aaa, bbb, ccc, X[ 6], 5);
FFF(ccc, ddd, aaa, bbb, X[ 4], 8);
FFF(bbb, ccc, ddd, aaa, X[ 1], 11);
FFF(aaa, bbb, ccc, ddd, X[ 3], 14);
FFF(ddd, aaa, bbb, ccc, X[11], 14);
FFF(ccc, ddd, aaa, bbb, X[15], 6);
FFF(bbb, ccc, ddd, aaa, X[ 0], 14);
FFF(aaa, bbb, ccc, ddd, X[ 5], 6);
FFF(ddd, aaa, bbb, ccc, X[12], 9);
FFF(ccc, ddd, aaa, bbb, X[ 2], 12);
FFF(bbb, ccc, ddd, aaa, X[13], 9);
FFF(aaa, bbb, ccc, ddd, X[ 9], 12);
FFF(ddd, aaa, bbb, ccc, X[ 7], 5);
FFF(ccc, ddd, aaa, bbb, X[10], 15);
FFF(bbb, ccc, ddd, aaa, X[14], 8);
/* combine results */
ddd += cc + md->rmd128.state[1]; /* final result for MDbuf[0] */
md->rmd128.state[1] = md->rmd128.state[2] + dd + aaa;
md->rmd128.state[2] = md->rmd128.state[3] + aa + bbb;
md->rmd128.state[3] = md->rmd128.state[0] + bb + ccc;
md->rmd128.state[0] = ddd;
return CRYPT_OK;
}
//对于每个block进行转换
void MD5::encryptWithBlock(const uint8_t block[64]) {
uint32_t a = state[0], b = state[1], c = state[2], d = state[3], x[16];
decode(block, x, 64);
/* Round 1 */
FF (a, b, c, d, x[ 0], S11, 0xd76aa478); /* 1 */
FF (d, a, b, c, x[ 1], S12, 0xe8c7b756); /* 2 */
FF (c, d, a, b, x[ 2], S13, 0x242070db); /* 3 */
FF (b, c, d, a, x[ 3], S14, 0xc1bdceee); /* 4 */
FF (a, b, c, d, x[ 4], S11, 0xf57c0faf); /* 5 */
FF (d, a, b, c, x[ 5], S12, 0x4787c62a); /* 6 */
FF (c, d, a, b, x[ 6], S13, 0xa8304613); /* 7 */
FF (b, c, d, a, x[ 7], S14, 0xfd469501); /* 8 */
FF (a, b, c, d, x[ 8], S11, 0x698098d8); /* 9 */
FF (d, a, b, c, x[ 9], S12, 0x8b44f7af); /* 10 */
FF (c, d, a, b, x[10], S13, 0xffff5bb1); /* 11 */
FF (b, c, d, a, x[11], S14, 0x895cd7be); /* 12 */
FF (a, b, c, d, x[12], S11, 0x6b901122); /* 13 */
FF (d, a, b, c, x[13], S12, 0xfd987193); /* 14 */
FF (c, d, a, b, x[14], S13, 0xa679438e); /* 15 */
FF (b, c, d, a, x[15], S14, 0x49b40821); /* 16 */
/* Round 2 */
GG (a, b, c, d, x[ 1], S21, 0xf61e2562); /* 17 */
GG (d, a, b, c, x[ 6], S22, 0xc040b340); /* 18 */
GG (c, d, a, b, x[11], S23, 0x265e5a51); /* 19 */
GG (b, c, d, a, x[ 0], S24, 0xe9b6c7aa); /* 20 */
GG (a, b, c, d, x[ 5], S21, 0xd62f105d); /* 21 */
GG (d, a, b, c, x[10], S22, 0x2441453); /* 22 */
GG (c, d, a, b, x[15], S23, 0xd8a1e681); /* 23 */
GG (b, c, d, a, x[ 4], S24, 0xe7d3fbc8); /* 24 */
GG (a, b, c, d, x[ 9], S21, 0x21e1cde6); /* 25 */
GG (d, a, b, c, x[14], S22, 0xc33707d6); /* 26 */
GG (c, d, a, b, x[ 3], S23, 0xf4d50d87); /* 27 */
GG (b, c, d, a, x[ 8], S24, 0x455a14ed); /* 28 */
GG (a, b, c, d, x[13], S21, 0xa9e3e905); /* 29 */
GG (d, a, b, c, x[ 2], S22, 0xfcefa3f8); /* 30 */
GG (c, d, a, b, x[ 7], S23, 0x676f02d9); /* 31 */
GG (b, c, d, a, x[12], S24, 0x8d2a4c8a); /* 32 */
/* Round 3 */
HH (a, b, c, d, x[ 5], S31, 0xfffa3942); /* 33 */
HH (d, a, b, c, x[ 8], S32, 0x8771f681); /* 34 */
HH (c, d, a, b, x[11], S33, 0x6d9d6122); /* 35 */
HH (b, c, d, a, x[14], S34, 0xfde5380c); /* 36 */
HH (a, b, c, d, x[ 1], S31, 0xa4beea44); /* 37 */
HH (d, a, b, c, x[ 4], S32, 0x4bdecfa9); /* 38 */
HH (c, d, a, b, x[ 7], S33, 0xf6bb4b60); /* 39 */
HH (b, c, d, a, x[10], S34, 0xbebfbc70); /* 40 */
HH (a, b, c, d, x[13], S31, 0x289b7ec6); /* 41 */
HH (d, a, b, c, x[ 0], S32, 0xeaa127fa); /* 42 */
HH (c, d, a, b, x[ 3], S33, 0xd4ef3085); /* 43 */
HH (b, c, d, a, x[ 6], S34, 0x4881d05); /* 44 */
HH (a, b, c, d, x[ 9], S31, 0xd9d4d039); /* 45 */
HH (d, a, b, c, x[12], S32, 0xe6db99e5); /* 46 */
HH (c, d, a, b, x[15], S33, 0x1fa27cf8); /* 47 */
HH (b, c, d, a, x[ 2], S34, 0xc4ac5665); /* 48 */
/* Round 4 */
II (a, b, c, d, x[ 0], S41, 0xf4292244); /* 49 */
II (d, a, b, c, x[ 7], S42, 0x432aff97); /* 50 */
II (c, d, a, b, x[14], S43, 0xab9423a7); /* 51 */
II (b, c, d, a, x[ 5], S44, 0xfc93a039); /* 52 */
II (a, b, c, d, x[12], S41, 0x655b59c3); /* 53 */
II (d, a, b, c, x[ 3], S42, 0x8f0ccc92); /* 54 */
II (c, d, a, b, x[10], S43, 0xffeff47d); /* 55 */
II (b, c, d, a, x[ 1], S44, 0x85845dd1); /* 56 */
II (a, b, c, d, x[ 8], S41, 0x6fa87e4f); /* 57 */
II (d, a, b, c, x[15], S42, 0xfe2ce6e0); /* 58 */
II (c, d, a, b, x[ 6], S43, 0xa3014314); /* 59 */
II (b, c, d, a, x[13], S44, 0x4e0811a1); /* 60 */
II (a, b, c, d, x[ 4], S41, 0xf7537e82); /* 61 */
II (d, a, b, c, x[11], S42, 0xbd3af235); /* 62 */
II (c, d, a, b, x[ 2], S43, 0x2ad7d2bb); /* 63 */
II (b, c, d, a, x[ 9], S44, 0xeb86d391); /* 64 */
state[0] += a;
state[1] += b;
state[2] += c;
state[3] += d;
}
