static void Transform(Sha* sha)
{
word32 W[80], i;
/* Copy context->state[] to working vars */
word32 a = sha->digest[0];
word32 b = sha->digest[1];
word32 c = sha->digest[2];
word32 d = sha->digest[3];
word32 e = sha->digest[4];
for (i = 0; i < 16; i++)
W[i] = sha->buffer[i];
for (i = 16; i < 80; i++)
W[i] = rotlFixed(W[i-3]^W[i-8]^W[i-14]^W[i-16],1);
/* 4 rounds of 20 operations each. */
for (i = 0; i < 20; ) {
R0(a,b,c,d,e,i); i++;
R0(e,a,b,c,d,i); i++;
R0(d,e,a,b,c,i); i++;
R0(c,d,e,a,b,i); i++;
R0(b,c,d,e,a,i); i++;
}
for (i = 20; i < 40; ) {
R2(a,b,c,d,e,i); i++;
R2(e,a,b,c,d,i); i++;
R2(d,e,a,b,c,i); i++;
R2(c,d,e,a,b,i); i++;
R2(b,c,d,e,a,i); i++;
}
for (i = 40; i < 60; ) {
R3(a,b,c,d,e,i); i++;
R3(e,a,b,c,d,i); i++;
R3(d,e,a,b,c,i); i++;
R3(c,d,e,a,b,i); i++;
R3(b,c,d,e,a,i); i++;
}
for (i = 60; i < 80; ) {
R4(a,b,c,d,e,i); i++;
R4(e,a,b,c,d,i); i++;
R4(d,e,a,b,c,i); i++;
R4(c,d,e,a,b,i); i++;
R4(b,c,d,e,a,i); i++;
}
/* Add the working vars back into digest state[] */
sha->digest[0] += a;
sha->digest[1] += b;
sha->digest[2] += c;
sha->digest[3] += d;
sha->digest[4] += e;
}
static void transform(uint32_t state[5], uint8_t buffer[64]){
uint32_t block[80];
unsigned int i, a, b, c, d, e;
a = state[0];
b = state[1];
c = state[2];
d = state[3];
e = state[4];
#ifdef CONFIG_SMALL
for(i=0; i<80; i++){
int t;
if(i<16) t= be2me_32(((uint32_t*)buffer)[i]);
else t= rol(block[i-3]^block[i-8]^block[i-14]^block[i-16],1);
block[i]= t;
t+= e+rol(a,5);
if(i<40){
if(i<20) t+= ((b&(c^d))^d) +0x5A827999;
else t+= ( b^c ^d) +0x6ED9EBA1;
}else{
if(i<60) t+= (((b|c)&d)|(b&c))+0x8F1BBCDC;
else t+= ( b^c ^d) +0xCA62C1D6;
}
e= d;
d= c;
c= rol(b,30);
b= a;
a= t;
}
#else
for(i=0; i<15; i+=5){
R0(a,b,c,d,e,0+i); R0(e,a,b,c,d,1+i); R0(d,e,a,b,c,2+i); R0(c,d,e,a,b,3+i); R0(b,c,d,e,a,4+i);
}
R0(a,b,c,d,e,15); R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
for(i=20; i<40; i+=5){
R2(a,b,c,d,e,0+i); R2(e,a,b,c,d,1+i); R2(d,e,a,b,c,2+i); R2(c,d,e,a,b,3+i); R2(b,c,d,e,a,4+i);
}
for(; i<60; i+=5){
R3(a,b,c,d,e,0+i); R3(e,a,b,c,d,1+i); R3(d,e,a,b,c,2+i); R3(c,d,e,a,b,3+i); R3(b,c,d,e,a,4+i);
}
for(; i<80; i+=5){
R4(a,b,c,d,e,0+i); R4(e,a,b,c,d,1+i); R4(d,e,a,b,c,2+i); R4(c,d,e,a,b,3+i); R4(b,c,d,e,a,4+i);
}
#endif
state[0] += a;
state[1] += b;
state[2] += c;
state[3] += d;
state[4] += e;
}
static bool k8_mc2_mce(u16 ec, u8 xec)
{
bool ret = true;
if (xec == 0x1)
pr_cont(" in the write data buffers.\n");
else if (xec == 0x3)
pr_cont(" in the victim data buffers.\n");
else if (xec == 0x2 && MEM_ERROR(ec))
pr_cont(": %s error in the L2 cache tags.\n", R4_MSG(ec));
else if (xec == 0x0) {
if (TLB_ERROR(ec))
pr_cont(": %s error in a Page Descriptor Cache or "
"Guest TLB.\n", TT_MSG(ec));
else if (BUS_ERROR(ec))
pr_cont(": %s/ECC error in data read from NB: %s.\n",
R4_MSG(ec), PP_MSG(ec));
else if (MEM_ERROR(ec)) {
u8 r4 = R4(ec);
if (r4 >= 0x7)
pr_cont(": %s error during data copyback.\n",
R4_MSG(ec));
else if (r4 <= 0x1)
pr_cont(": %s parity/ECC error during data "
"access from L2.\n", R4_MSG(ec));
else
ret = false;
} else
ret = false;
} else
ret = false;
return ret;
}
void Homography::estimate(int *noi, int *nof, double threshold)
{
if(_n < 20) {
_hom.setIdentity();
return;
}
RX::mat3 H3, H4;
Ransac R4(_ux2, _ux1, _nused);
R4.normalize();
R4.mainLoop(4, 0.01, H4);
R4.fitHomography(H4);
R4.denormalize(H4);
_hom = H4;
if(noi) *noi = R4.noi();
if(nof) *nof = R4.nof();
_inliers = R4.inliers();
for(int i = 0; i < 3; ++i) {
for(int j = 0; j < 3; ++j) {
_hom.set(i, j, _hom.at(i, j)/_hom.at(2, 2));
}
}
}
static void sha1_core(struct sha1_ctx * ctx)
{
__m128i a, b, c, d, e;
__m128i k0, k1, k2, k3;
__m128i v1, w;
SVAL buf[16];
a = SET1(AA);
b = SET1(BB);
c = SET1(CC);
d = SET1(DD);
e = SET1(EE);
k0 = SET1(K0);
R20(SHA1R0, 0);
k1 = SET1(K1);
R20(SHA1R1, 20);
k2 = SET1(K2);
R20(SHA1R2, 40);
k3 = SET1(K3);
R16(SHA1R3, 60);
if (!ssresult_check(ctx->sres, a)) return;
R4(SHA1R3, 76);
FINAL(0, a, AA);
FINAL(1, b, BB);
FINAL(2, c, CC);
FINAL(3, d, DD);
FINAL(4, e, EE);
check_result(ctx);
}
static bool k8_mc1_mce(u16 ec, u8 xec)
{
u8 ll = LL(ec);
bool ret = true;
if (!MEM_ERROR(ec))
return false;
if (ll == 0x2)
pr_cont("during a linefill from L2.\n");
else if (ll == 0x1) {
switch (R4(ec)) {
case R4_IRD:
pr_cont("Parity error during data load.\n");
break;
case R4_EVICT:
pr_cont("Copyback Parity/Victim error.\n");
break;
case R4_SNOOP:
pr_cont("Tag Snoop error.\n");
break;
default:
ret = false;
break;
}
} else
ret = false;
return ret;
}
static void decode_mc3_mce(struct mce *m)
{
u16 ec = EC(m->status);
u8 xec = XEC(m->status, xec_mask);
if (boot_cpu_data.x86 >= 0x14) {
pr_emerg("You shouldn't be seeing MC3 MCE on this cpu family,"
" please report on LKML.\n");
return;
}
pr_emerg(HW_ERR "MC3 Error");
if (xec == 0x0) {
u8 r4 = R4(ec);
if (!BUS_ERROR(ec) || (r4 != R4_DRD && r4 != R4_DWR))
goto wrong_mc3_mce;
pr_cont(" during %s.\n", R4_MSG(ec));
} else
goto wrong_mc3_mce;
return;
wrong_mc3_mce:
pr_emerg(HW_ERR "Corrupted MC3 MCE info?\n");
}
static bool f10h_mc0_mce(u16 ec, u8 xec)
{
if (R4(ec) == R4_GEN && LL(ec) == LL_L1) {
pr_cont("during data scrub.\n");
return true;
}
return f12h_mc0_mce(ec, xec);
}
void
EulerAngleUpdaterCheck::initialize()
{
const auto grain_num = _grain_tracker.getTotalFeatureCount();
_angles.resize(grain_num);
_angles_old.resize(grain_num);
_diff.assign(3 * grain_num, 0.0);
for (unsigned int i = 0; i < grain_num; ++i)
{
_angles[i] = _euler.getEulerAngles(i);
_angles_old[i] = _euler.getEulerAnglesOld(i);
RealGradient torque = _grain_torque.getTorqueValues()[i];
RealVectorValue a(1, 1, 1);
RotationTensor R(_angles[i]); // Final rotation tensor
RealVectorValue a_rot = R * a; // final rotated vector
RotationTensor R0(_angles_old[i]); // RotationTensor as per old euler angles
RealVectorValue torque_rot = R0 * torque; // Rotated torque
RealVectorValue a_rot0 = R0 * a; // Rotated unit vector as per old euler angles
/**
* Change in euler angles are obtained from the torque & angular velocities about the material
* axes.
* Change in phi1, Phi and phi2 are caused by rotation about z axis, x' axis & z'' axis,
* respectively.
* Components of the angular velocities across z, x' and z'' axes are obtained from the torque
* values.
* This yields change in euler angles due to grain rotation.
*/
RealVectorValue torque_rot1;
RealVectorValue angle_rot;
torque_rot1(0) =
torque_rot(2); // Tourque about z changed to torque responsible for chaneg in angle phi1
angle_rot(0) = _mr / _grain_volumes[i] * torque_rot1(0) * _dt; // change in phi1
// Tourque about x' changed to torque responsible for chaneg in angle Phi
torque_rot1(1) =
(torque_rot(0) * std::cos(angle_rot(0)) + torque_rot(1) * std::sin(angle_rot(0)));
angle_rot(1) = _mr / _grain_volumes[i] * torque_rot1(1) * _dt; // change in Phi
// Tourque about z'' changed to torque responsible for chaneg in angle phi2
torque_rot1(2) = (torque_rot(0) * std::sin(angle_rot(0)) * std::sin(angle_rot(1)) -
torque_rot(1) * std::cos(angle_rot(0)) * std::sin(angle_rot(1)) +
torque_rot(2) * std::cos(angle_rot(1)));
angle_rot(2) = _mr / _grain_volumes[i] * torque_rot1(2) * _dt; // change in phi2
angle_rot *= (180.0 / libMesh::pi);
RotationTensor R4(angle_rot); // RotationTensor due to grain rotation
RealVectorValue a_rot1 = R4 * a_rot0; // Final rotated vector obtained in two step rotation
// Difference between the final positions of the rotated vector obtained in two different ways,
// should be 0.0
_diff[3 * i + 0] = a_rot(0) - a_rot1(0);
_diff[3 * i + 1] = a_rot(1) - a_rot1(1);
_diff[3 * i + 2] = a_rot(2) - a_rot1(2);
}
}
int main(int argc, char** argv) {
Giornale G1(12.50, "Titolo1", false);
Giornale G2(1.50, "Titolo2", true);
Giornale G3(2.00, "Titolo3", false);
Rivista R4(22.70, "Titolo4", false, "Editore4", "Periodo4");
Rivista R5(11.50, "Titolo5", true, "Editore5", "Periodo5");
Rivista R6(6.00, "Titolo6", false, "Editore6", "Periodo6");
Quotidiano Q7(6.35, "Titolo7", false, "Direttore7", true);
Quotidiano Q8(9.99, "Titolo8", true, "Direttore8", false);
Quotidiano Q9(5, "Titolo9", false, "Direttore9", true);
cout<<"Polimorfismo:\n";
Giornale * vett[9];
vett[0] = &G1;
vett[1] = &G2;
vett[2] = &G3;
vett[3] = &R4;
vett[4] = &R5;
vett[5] = &R6;
vett[6] = &Q7;
vett[7] = &Q8;
vett[8] = &Q9;
for(int i=0; i<9; i++) {
cout << *vett[i] << "\n\n";
}
cout<<"\n\nPila:\n";
Pila P;
for(int i=0; i<9; i++) {
P.push(vett[i]);
}
cout<<P;
ofstream file;
file.open("./test.txt", ios::out);
if(!file) {
cout<<"Errore apertura file.";
} else {
file << P;
}
file.close();
Rivista R10(1.00, "Titolo10", false, "Editore10", "Periodo10");
Quotidiano Q11(1.35, "Titolo11", false, "Direttore11", true);
P.push(&R10);
cout<<"\n\nEccezione:\n";
try {
P.push(&Q11);
} catch(SpaceOverflow e) {
cout<<e.errorLog();
}
return 0;
}
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;
}
/* decode skytraq raw channel mesurement -------------------------------------*/
static int decode_stqraw(raw_t *raw)
{
int i,j,iod,prn,sat,n=0,nsat;
unsigned char *p=raw->buff+4,ind;
trace(4,"decode_stqraw: len=%d\n",raw->len);
iod=U1(p+1);
if (iod!=raw->iod) {
trace(2,"stq raw iod error: iod=%d %d\n",iod,raw->iod);
return -1;
}
nsat=U1(p+2);
if (raw->len<8+23*nsat) {
trace(2,"stq raw length error: len=%d nsat=%d\n",raw->len,nsat);
return -1;
}
for (i=0,p+=3;i<nsat&&i<MAXOBS;i++,p+=23) {
ind =U1(p+22);
prn =U1(p);
raw->obs.data[n].SNR[0]=(unsigned char)(U1(p+1)*4.0+0.5);
raw->obs.data[n].P[0] =(ind&0x1)?R8(p+ 2):0.0;
raw->obs.data[n].L[0] =(ind&0x4)?R8(p+10):0.0;
raw->obs.data[n].D[0] =(ind&0x2)?R4(p+18):0.0f;
raw->obs.data[n].LLI[0]=(ind&0x8)?1:0; /* slip */
raw->obs.data[n].code[0]=CODE_L1C;
/* receiver dependent options */
if (strstr(raw->opt,"-invcp")) {
raw->obs.data[n].L[0]=-raw->obs.data[n].L[0];
}
if (prn>MAXPRNGPS) prn+=MINPRNSBS-38;
if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
trace(2,"stq raw satellite number error: prn=%d\n",prn);
continue;
}
raw->obs.data[n].time=raw->time;
raw->obs.data[n].sat =sat;
for (j=1;j<NFREQ;j++) {
raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0;
raw->obs.data[n].D[j]=0.0;
raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0;
raw->obs.data[n].code[j]=CODE_NONE;
}
n++;
}
raw->obs.n=n;
return n>0?1:0;
}
/* decode NVS x4aiono --------------------------------------------------------*/
static int decode_x4aiono(raw_t *raw)
{
unsigned char *p=raw->buff+2;
trace(4,"decode_x4aiono: len=%d\n", raw->len);
raw->nav.ion_gps[0] = R4(p );
raw->nav.ion_gps[1] = R4(p+ 4);
raw->nav.ion_gps[2] = R4(p+ 8);
raw->nav.ion_gps[3] = R4(p+12);
raw->nav.ion_gps[4] = R4(p+16);
raw->nav.ion_gps[5] = R4(p+20);
raw->nav.ion_gps[6] = R4(p+24);
raw->nav.ion_gps[7] = R4(p+28);
return 9;
}
/* decode binex mesaage 0x01-03: decoded sbas ephmemeris ---------------------*/
static int decode_bnx_01_03(raw_t *raw, unsigned char *buff, int len)
{
seph_t seph={0};
unsigned char *p=buff;
double tow,tod,tof;
int prn,week,iodn;
trace(4,"binex 0x01-03: len=%d\n",len);
if (len>=98) {
prn =U1(p); p+=1;
week =U2(p); p+=2;
tow =U4(p); p+=4;
seph.af0 =R8(p); p+=8;
tod =R4(p); p+=4;
tof =U4(p); p+=4;
seph.pos[0]=R8(p)*1E3; p+=8;
seph.vel[0]=R8(p)*1E3; p+=8;
seph.acc[0]=R8(p)*1E3; p+=8;
seph.pos[1]=R8(p)*1E3; p+=8;
seph.vel[1]=R8(p)*1E3; p+=8;
seph.acc[1]=R8(p)*1E3; p+=8;
seph.pos[2]=R8(p)*1E3; p+=8;
seph.vel[2]=R8(p)*1E3; p+=8;
seph.acc[2]=R8(p)*1E3; p+=8;
seph.svh =U1(p); p+=1;
seph.sva =U1(p); p+=1;
iodn =U1(p);
}
else {
trace(2,"binex 0x01-03 length error: len=%d\n",len);
return -1;
}
if (!(seph.sat=satno(SYS_SBS,prn))) {
trace(2,"binex 0x01-03 satellite error: prn=%d\n",prn);
return -1;
}
seph.t0=gpst2time(week,tow);
seph.tof=adjweek(seph.t0,tof);
if (!strstr(raw->opt,"-EPHALL")) {
if (fabs(timediff(seph.t0,raw->nav.seph[prn-MINPRNSBS].t0))<1.0&&
seph.sva==raw->nav.seph[prn-MINPRNSBS].sva) return 0; /* unchanged */
}
raw->nav.seph[prn-MINPRNSBS]=seph;
raw->ephsat=seph.sat;
return 2;
}
static bool f14h_mc1_mce(u16 ec, u8 xec)
{
u8 r4 = R4(ec);
bool ret = true;
if (MEM_ERROR(ec)) {
if (TT(ec) != 0 || LL(ec) != 1)
ret = false;
if (r4 == R4_IRD)
pr_cont("Data/tag array parity error for a tag hit.\n");
else if (r4 == R4_SNOOP)
pr_cont("Tag error during snoop/victimization.\n");
else
ret = false;
}
return ret;
}
static void decode_mc2_mce(struct mce *m)
{
u16 ec = EC(m->status);
u8 xec = XEC(m->status, xec_mask);
pr_emerg(HW_ERR "MC2 Error");
if (xec == 0x1)
pr_cont(" in the write data buffers.\n");
else if (xec == 0x3)
pr_cont(" in the victim data buffers.\n");
else if (xec == 0x2 && MEM_ERROR(ec))
pr_cont(": %s error in the L2 cache tags.\n", R4_MSG(ec));
else if (xec == 0x0) {
if (TLB_ERROR(ec))
pr_cont(": %s error in a Page Descriptor Cache or "
"Guest TLB.\n", TT_MSG(ec));
else if (BUS_ERROR(ec))
pr_cont(": %s/ECC error in data read from NB: %s.\n",
R4_MSG(ec), PP_MSG(ec));
else if (MEM_ERROR(ec)) {
u8 r4 = R4(ec);
if (r4 >= 0x7)
pr_cont(": %s error during data copyback.\n",
R4_MSG(ec));
else if (r4 <= 0x1)
pr_cont(": %s parity/ECC error during data "
"access from L2.\n", R4_MSG(ec));
else
goto wrong_mc2_mce;
} else
goto wrong_mc2_mce;
} else
goto wrong_mc2_mce;
return;
wrong_mc2_mce:
pr_emerg(HW_ERR "Corrupted MC2 MCE info?\n");
}
static bool f16h_mc2_mce(u16 ec, u8 xec)
{
u8 r4 = R4(ec);
if (!MEM_ERROR(ec))
return false;
switch (xec) {
case 0x04 ... 0x05:
pr_cont("%cBUFF parity error.\n", (r4 == R4_RD) ? 'I' : 'O');
break;
case 0x09 ... 0x0b:
case 0x0d ... 0x0f:
pr_cont("ECC error in L2 tag (%s).\n",
((r4 == R4_GEN) ? "BankReq" :
((r4 == R4_SNOOP) ? "Prb" : "Fill")));
break;
case 0x10 ... 0x19:
case 0x1b:
pr_cont("ECC error in L2 data array (%s).\n",
(((r4 == R4_RD) && !(xec & 0x3)) ? "Hit" :
((r4 == R4_GEN) ? "Attr" :
((r4 == R4_EVICT) ? "Vict" : "Fill"))));
break;
case 0x1c ... 0x1d:
case 0x1f:
pr_cont("Parity error in L2 attribute bits (%s).\n",
((r4 == R4_RD) ? "Hit" :
((r4 == R4_GEN) ? "Attr" : "Fill")));
break;
default:
return false;
}
return true;
}
RX::mat3 Homography::estimatePriv()
{
RX::mat3 H3, H4, Hn;
Ransac R4(_ux2, _ux1, _nused);
R4.normalize();
R4.mainLoop(4, 0.01, H4);
R4.fitHomography(H4);
R4.denormalize(H4);
//Ransac R3(_ux2, _ux1, _nused);
//R3.normalize();
//R3.mainLoop(4, 0.0001, H3);
//R3.fitHomography(H3);
//R3.denormalize(H3);
//
//if(R3.noi() >= 2*R4.noi())
//Hn = H3;
//else
Hn = H4;
return Hn;
}
void Homography::estimate(int *noi, int *nof, double threshold)
{
RX::mat3 H3, H4;
Ransac R4(_ux2, _ux1, _nused);
R4.normalize();
R4.mainLoop(4, 0.01, H4);
R4.fitHomography(H4);
R4.denormalize(H4);
//Ransac R3(_ux2, _ux1, _nused);
//R3.normalize();
//R3.mainLoop(4, 0.1, H3);
//R3.fitHomography(H3);
//R3.denormalize(H3);
//if(R3.noi() >= 2*R4.noi())
//{
// _hom = H3;
// if(noi) *noi = R3.noi();
// if(nof) *nof = R3.nof();
// _inliers = R3.inliers();
//}
//else
//{
_hom = H4;
if(noi) *noi = R4.noi();
if(nof) *nof = R4.nof();
_inliers = R4.inliers();
//}
for(int i = 0; i < 3; ++i) {
for(int j = 0; j < 3; ++j) {
_hom.set(i, j, _hom.at(i, j)/_hom.at(2, 2));
}
}
}
static bool cat_mc1_mce(u16 ec, u8 xec)
{
u8 r4 = R4(ec);
bool ret = true;
if (!MEM_ERROR(ec))
return false;
if (TT(ec) != TT_INSTR)
return false;
if (r4 == R4_IRD)
pr_cont("Data/tag array parity error for a tag hit.\n");
else if (r4 == R4_SNOOP)
pr_cont("Tag error during snoop/victimization.\n");
else if (xec == 0x0)
pr_cont("Tag parity error from victim castout.\n");
else if (xec == 0x2)
pr_cont("Microcode patch RAM parity error.\n");
else
ret = false;
return ret;
}
void sha1_update(sha1_ctx_t *ctx, void *data, size_t dlen) {
uint16_t avail; /* space available in the context message block */
uint16_t pos; /* position to write data into the message block */
uint32_t t;
uint32_t x[80];
while (dlen) {
pos = CTX_POS(ctx);
avail = CTX_AVAIL(ctx);
if (avail > dlen)
avail = dlen;
/* Copy input data into the context's message block. */
memcpy(ctx->block + pos, data, avail);
data += avail;
dlen -= avail;
ctx->len += avail;
if ((ctx->len % 64) == 0) {
uint32_t a = ctx->hash[0];
uint32_t b = ctx->hash[1];
uint32_t c = ctx->hash[2];
uint32_t d = ctx->hash[3];
uint32_t e = ctx->hash[4];
R1a(t, a, b, c, d, e, x, 0);
R1a(t, a, b, c, d, e, x, 1);
R1a(t, a, b, c, d, e, x, 2);
R1a(t, a, b, c, d, e, x, 3);
R1a(t, a, b, c, d, e, x, 4);
R1a(t, a, b, c, d, e, x, 5);
R1a(t, a, b, c, d, e, x, 6);
R1a(t, a, b, c, d, e, x, 7);
R1a(t, a, b, c, d, e, x, 8);
R1a(t, a, b, c, d, e, x, 9);
R1a(t, a, b, c, d, e, x, 10);
R1a(t, a, b, c, d, e, x, 11);
R1a(t, a, b, c, d, e, x, 12);
R1a(t, a, b, c, d, e, x, 13);
R1a(t, a, b, c, d, e, x, 14);
R1a(t, a, b, c, d, e, x, 15);
R1b(t, a, b, c, d, e, x, 16);
R1b(t, a, b, c, d, e, x, 17);
R1b(t, a, b, c, d, e, x, 18);
R1b(t, a, b, c, d, e, x, 19);
R2(t, a, b, c, d, e, x, 20);
R2(t, a, b, c, d, e, x, 21);
R2(t, a, b, c, d, e, x, 22);
R2(t, a, b, c, d, e, x, 23);
R2(t, a, b, c, d, e, x, 24);
R2(t, a, b, c, d, e, x, 25);
R2(t, a, b, c, d, e, x, 26);
R2(t, a, b, c, d, e, x, 27);
R2(t, a, b, c, d, e, x, 28);
R2(t, a, b, c, d, e, x, 29);
R2(t, a, b, c, d, e, x, 30);
R2(t, a, b, c, d, e, x, 31);
R2(t, a, b, c, d, e, x, 32);
R2(t, a, b, c, d, e, x, 33);
R2(t, a, b, c, d, e, x, 34);
R2(t, a, b, c, d, e, x, 35);
R2(t, a, b, c, d, e, x, 36);
R2(t, a, b, c, d, e, x, 37);
R2(t, a, b, c, d, e, x, 38);
R2(t, a, b, c, d, e, x, 39);
R3(t, a, b, c, d, e, x, 40);
R3(t, a, b, c, d, e, x, 41);
R3(t, a, b, c, d, e, x, 42);
R3(t, a, b, c, d, e, x, 43);
R3(t, a, b, c, d, e, x, 44);
R3(t, a, b, c, d, e, x, 45);
R3(t, a, b, c, d, e, x, 46);
R3(t, a, b, c, d, e, x, 47);
R3(t, a, b, c, d, e, x, 48);
R3(t, a, b, c, d, e, x, 49);
R3(t, a, b, c, d, e, x, 50);
R3(t, a, b, c, d, e, x, 51);
R3(t, a, b, c, d, e, x, 52);
R3(t, a, b, c, d, e, x, 53);
R3(t, a, b, c, d, e, x, 54);
R3(t, a, b, c, d, e, x, 55);
R3(t, a, b, c, d, e, x, 56);
R3(t, a, b, c, d, e, x, 57);
R3(t, a, b, c, d, e, x, 58);
R3(t, a, b, c, d, e, x, 59);
R4(t, a, b, c, d, e, x, 60);
R4(t, a, b, c, d, e, x, 61);
R4(t, a, b, c, d, e, x, 62);
R4(t, a, b, c, d, e, x, 63);
R4(t, a, b, c, d, e, x, 64);
R4(t, a, b, c, d, e, x, 65);
R4(t, a, b, c, d, e, x, 66);
R4(t, a, b, c, d, e, x, 67);
R4(t, a, b, c, d, e, x, 68);
R4(t, a, b, c, d, e, x, 69);
R4(t, a, b, c, d, e, x, 70);
R4(t, a, b, c, d, e, x, 71);
R4(t, a, b, c, d, e, x, 72);
R4(t, a, b, c, d, e, x, 73);
R4(t, a, b, c, d, e, x, 74);
R4(t, a, b, c, d, e, x, 75);
R4(t, a, b, c, d, e, x, 76);
R4(t, a, b, c, d, e, x, 77);
R4(t, a, b, c, d, e, x, 78);
R4(t, a, b, c, d, e, x, 79);
ctx->hash[0] += a;
ctx->hash[1] += b;
ctx->hash[2] += c;
ctx->hash[3] += d;
ctx->hash[4] += e;
}
}
}
/* decode binex mesaage 0x01-05: decoded beidou-2/compass ephmemeris ---------*/
static int decode_bnx_01_05(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,toc,sqrtA;
int prn,flag1,flag2;
trace(4,"binex 0x01-05: len=%d\n",len);
if (len>=117) {
prn =U1(p); p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
toc =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.f2 =R4(p); p+=4;
eph.f1 =R4(p); p+=4;
eph.f0 =R4(p); p+=4;
eph.deln =R4(p)*SC2RAD; p+=4;
eph.M0 =R8(p); p+=8;
eph.e =R8(p); p+=8;
sqrtA =R8(p); p+=8;
eph.cic =R4(p); p+=4;
eph.crc =R4(p); p+=4;
eph.cis =R4(p); p+=4;
eph.crs =R4(p); p+=4;
eph.cuc =R4(p); p+=4;
eph.cus =R4(p); p+=4;
eph.OMG0 =R8(p); p+=8;
eph.omg =R8(p); p+=8;
eph.i0 =R8(p); p+=8;
eph.OMGd =R4(p)*SC2RAD; p+=4;
eph.idot =R4(p)*SC2RAD; p+=4;
flag1 =U2(p); p+=2;
flag2 =U4(p);
}
else {
trace(2,"binex 0x01-05: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_CMP,prn))) {
trace(2,"binex 0x01-05: satellite error prn=%d\n",prn);
return 0;
}
eph.A=sqrtA*sqrtA;
eph.toe=gpst2time(eph.week+1356,eph.toes+14.0); /* bdt -> gpst */
eph.toc=gpst2time(eph.week+1356,eph.toes+14.0); /* bdt -> gpst */
eph.ttr=adjweek(eph.toe,tow+14.0); /* bdt -> gpst */
eph.iodc=(flag1>>1)&0x1F;
eph.iode=(flag1>>6)&0x1F;
eph.svh=flag1&0x01;
eph.sva=flag2&0x0F; /* ura index */
eph.tgd[0]=bds_tgd(flag2>> 4); /* TGD1 (s) */
eph.tgd[1]=bds_tgd(flag2>>14); /* TGD2 (s) */
eph.flag=(flag1>>11)&0x07; /* nav type (0:unknown,1:IGSO/MEO,2:GEO) */
eph.code=(flag2>>25)&0x7F;
/* message source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)*/
if (!strstr(raw->opt,"-EPHALL")) {
if (raw->nav.eph[eph.sat-1].iode==eph.iode&&
raw->nav.eph[eph.sat-1].iodc==eph.iodc) return 0; /* unchanged */
}
raw->nav.eph[eph.sat-1]=eph;
raw->ephsat=eph.sat;
return 2;
}
/* decode binex mesaage 0x01-06: decoded qzss ephmemeris ---------------------*/
static int decode_bnx_01_06(raw_t *raw, unsigned char *buff, int len)
{
eph_t eph={0};
unsigned char *p=buff;
double tow,ura,sqrtA;
int prn,flag;
trace(4,"binex 0x01-06: len=%d\n",len);
if (len>=127) {
prn =U1(p); p+=1;
eph.week =U2(p); p+=2;
tow =I4(p); p+=4;
eph.toes =I4(p); p+=4;
eph.tgd[0]=R4(p); p+=4;
eph.iodc =I4(p); p+=4;
eph.f2 =R4(p); p+=4;
eph.f1 =R4(p); p+=4;
eph.f0 =R4(p); p+=4;
eph.iode =I4(p); p+=4;
eph.deln =R4(p)*SC2RAD; p+=4;
eph.M0 =R8(p); p+=8;
eph.e =R8(p); p+=8;
sqrtA =R8(p); p+=8;
eph.cic =R4(p); p+=4;
eph.crc =R4(p); p+=4;
eph.cis =R4(p); p+=4;
eph.crs =R4(p); p+=4;
eph.cuc =R4(p); p+=4;
eph.cus =R4(p); p+=4;
eph.OMG0 =R8(p); p+=8;
eph.omg =R8(p); p+=8;
eph.i0 =R8(p); p+=8;
eph.OMGd =R4(p)*SC2RAD; p+=4;
eph.idot =R4(p)*SC2RAD; p+=4;
ura =R4(p)*0.1; p+=4;
eph.svh =U2(p); p+=2;
flag =U2(p);
}
else {
trace(2,"binex 0x01-06: length error len=%d\n",len);
return -1;
}
if (!(eph.sat=satno(SYS_QZS,prn))) {
trace(2,"binex 0x01-06: satellite error prn=%d\n",prn);
return 0;
}
eph.A=sqrtA*sqrtA;
eph.toe=gpst2time(eph.week,eph.toes);
eph.toc=gpst2time(eph.week,eph.toes);
eph.ttr=adjweek(eph.toe,tow);
eph.fit=(flag&0x01)?0.0:2.0; /* 0:2hr,1:>2hr */
eph.sva=uraindex(ura);
eph.code=2; /* codes on L2 channel */
if (!strstr(raw->opt,"-EPHALL")) {
if (raw->nav.eph[eph.sat-1].iode==eph.iode&&
raw->nav.eph[eph.sat-1].iodc==eph.iodc) return 0; /* unchanged */
}
raw->nav.eph[eph.sat-1]=eph;
raw->ephsat=eph.sat;
return 2;
}
/* b: message to decode, len: length of message, h: where to output digest */
void md5(uchar *b,int len,uchar *h) {
static uchar M[16384],xx[16];
unsigned *X=(unsigned *)xx;
int l=len,i,j;
unsigned A=0x67452301,B=0xefcdab89,C=0x98badcfe,D=0x10325476,AA,BB,CC,DD;
if(len>16384-64) { h[0]=0; return; }
memcpy(M,b,len);
for(M[len++]=128;(len&63)!=56;) M[len++]=0;
M[len]=(l*8)&255; M[len+1]=((l*8)>>8)&255; M[len+2]=((l*8)>>16)&255; M[len+3]=(l*8)>>24;
M[len+4]=M[len+5]=M[len+6]=M[len+7]=0;
len+=8;
for(i=0;i<len;i+=64) {
for(j=0;j<64;j++) xx[j]=M[i+j];
AA=A,BB=B,CC=C,DD=D;
R1(A,B,C,D, 0,7,T[ 0]); R1(D,A,B,C, 1,12,T[ 1]); R1(C,D,A,B, 2,17,T[ 2]); R1(B,C,D,A, 3,22,T[ 3]);
R1(A,B,C,D, 4,7,T[ 4]); R1(D,A,B,C, 5,12,T[ 5]); R1(C,D,A,B, 6,17,T[ 6]); R1(B,C,D,A, 7,22,T[ 7]);
R1(A,B,C,D, 8,7,T[ 8]); R1(D,A,B,C, 9,12,T[ 9]); R1(C,D,A,B,10,17,T[10]); R1(B,C,D,A,11,22,T[11]);
R1(A,B,C,D,12,7,T[12]); R1(D,A,B,C,13,12,T[13]); R1(C,D,A,B,14,17,T[14]); R1(B,C,D,A,15,22,T[15]);
R2(A,B,C,D, 1,5,T[16]); R2(D,A,B,C, 6, 9,T[17]); R2(C,D,A,B,11,14,T[18]); R2(B,C,D,A, 0,20,T[19]);
R2(A,B,C,D, 5,5,T[20]); R2(D,A,B,C,10, 9,T[21]); R2(C,D,A,B,15,14,T[22]); R2(B,C,D,A, 4,20,T[23]);
R2(A,B,C,D, 9,5,T[24]); R2(D,A,B,C,14, 9,T[25]); R2(C,D,A,B, 3,14,T[26]); R2(B,C,D,A, 8,20,T[27]);
R2(A,B,C,D,13,5,T[28]); R2(D,A,B,C, 2, 9,T[29]); R2(C,D,A,B, 7,14,T[30]); R2(B,C,D,A,12,20,T[31]);
R3(A,B,C,D, 5,4,T[32]); R3(D,A,B,C, 8,11,T[33]); R3(C,D,A,B,11,16,T[34]); R3(B,C,D,A,14,23,T[35]);
R3(A,B,C,D, 1,4,T[36]); R3(D,A,B,C, 4,11,T[37]); R3(C,D,A,B, 7,16,T[38]); R3(B,C,D,A,10,23,T[39]);
R3(A,B,C,D,13,4,T[40]); R3(D,A,B,C, 0,11,T[41]); R3(C,D,A,B, 3,16,T[42]); R3(B,C,D,A, 6,23,T[43]);
R3(A,B,C,D, 9,4,T[44]); R3(D,A,B,C,12,11,T[45]); R3(C,D,A,B,15,16,T[46]); R3(B,C,D,A, 2,23,T[47]);
R4(A,B,C,D, 0,6,T[48]); R4(D,A,B,C, 7,10,T[49]); R4(C,D,A,B,14,15,T[50]); R4(B,C,D,A, 5,21,T[51]);
R4(A,B,C,D,12,6,T[52]); R4(D,A,B,C, 3,10,T[53]); R4(C,D,A,B,10,15,T[54]); R4(B,C,D,A, 1,21,T[55]);
R4(A,B,C,D, 8,6,T[56]); R4(D,A,B,C,15,10,T[57]); R4(C,D,A,B, 6,15,T[58]); R4(B,C,D,A,13,21,T[59]);
R4(A,B,C,D, 4,6,T[60]); R4(D,A,B,C,11,10,T[61]); R4(C,D,A,B, 2,15,T[62]); R4(B,C,D,A, 9,21,T[63]);
A+=AA,B+=BB,C+=CC,D+=DD;
}
h[0]=A&255; h[1]=(A>>8)&255; h[2]=(A>>16)&255; h[3]=A>>24;
h[4]=B&255; h[5]=(B>>8)&255; h[6]=(B>>16)&255; h[7]=B>>24;
h[8]=C&255; h[9]=(C>>8)&255; h[10]=(C>>16)&255; h[11]=C>>24;
h[12]=D&255; h[13]=(D>>8)&255; h[14]=(D>>16)&255; h[15]=D>>24;
}
void sha1_transform::transform(hash_word * state, const hash_word * data) {
#define blk0(i) (W[i] = data[i])
#define blk1(i) (W[i & 15] = rotl_fixed(W[(i + 13) & 15] ^ W[(i + 8) & 15] \
^ W[(i + 2) & 15] ^ W[i & 15], 1))
#define f1(x, y, z) (z ^ (x & (y ^ z)))
#define f2(x, y, z) (x ^ y ^ z)
#define f3(x, y, z) ((x & y) | (z & (x | y)))
#define f4(x, y, z) (x ^ y ^ z)
/* (R0+R1), R2, R3, R4 are the different operations used in SHA1 */
#define R0(v, w, x, y, z, i) z += f1(w, x, y) + blk0(i) + 0x5A827999 + rotl_fixed(v, 5); \
w = rotl_fixed(w, 30);
#define R1(v, w, x, y, z, i) z += f1(w, x, y) + blk1(i) + 0x5A827999 + rotl_fixed(v, 5); \
w = rotl_fixed(w, 30);
#define R2(v, w, x, y, z, i) z += f2(w, x, y) + blk1(i) + 0x6ED9EBA1 + rotl_fixed(v, 5); \
w = rotl_fixed(w, 30);
#define R3(v, w, x, y, z, i) z += f3(w, x, y) + blk1(i) + 0x8F1BBCDC + rotl_fixed(v, 5); \
w = rotl_fixed(w, 30);
#define R4(v, w, x, y, z, i) z += f4(w, x, y) + blk1(i) + 0xCA62C1D6 + rotl_fixed(v, 5); \
w = rotl_fixed(w, 30);
hash_word W[16];
/* Copy context->state[] to working vars */
hash_word a = state[0];
hash_word b = state[1];
hash_word c = state[2];
hash_word d = state[3];
hash_word e = state[4];
/* 4 rounds of 20 operations each. Loop unrolled. */
R0(a, b, c, d, e, 0);
R0(e, a, b, c, d, 1);
R0(d, e, a, b, c, 2);
R0(c, d, e, a, b, 3);
R0(b, c, d, e, a, 4);
R0(a, b, c, d, e, 5);
R0(e, a, b, c, d, 6);
R0(d, e, a, b, c, 7);
R0(c, d, e, a, b, 8);
R0(b, c, d, e, a, 9);
R0(a, b, c, d, e, 10);
R0(e, a, b, c, d, 11);
R0(d, e, a, b, c, 12);
R0(c, d, e, a, b, 13);
R0(b, c, d, e, a, 14);
R0(a, b, c, d, e, 15);
R1(e, a, b, c, d, 16);
R1(d, e, a, b, c, 17);
R1(c, d, e, a, b, 18);
R1(b, c, d, e, a, 19);
R2(a, b, c, d, e, 20);
R2(e, a, b, c, d, 21);
R2(d, e, a, b, c, 22);
R2(c, d, e, a, b, 23);
R2(b, c, d, e, a, 24);
R2(a, b, c, d, e, 25);
R2(e, a, b, c, d, 26);
R2(d, e, a, b, c, 27);
R2(c, d, e, a, b, 28);
R2(b, c, d, e, a, 29);
R2(a, b, c, d, e, 30);
R2(e, a, b, c, d, 31);
R2(d, e, a, b, c, 32);
R2(c, d, e, a, b, 33);
R2(b, c, d, e, a, 34);
R2(a, b, c, d, e, 35);
R2(e, a, b, c, d, 36);
R2(d, e, a, b, c, 37);
R2(c, d, e, a, b, 38);
R2(b, c, d, e, a, 39);
R3(a, b, c, d, e, 40);
R3(e, a, b, c, d, 41);
R3(d, e, a, b, c, 42);
R3(c, d, e, a, b, 43);
R3(b, c, d, e, a, 44);
R3(a, b, c, d, e, 45);
R3(e, a, b, c, d, 46);
R3(d, e, a, b, c, 47);
R3(c, d, e, a, b, 48);
R3(b, c, d, e, a, 49);
R3(a, b, c, d, e, 50);
R3(e, a, b, c, d, 51);
R3(d, e, a, b, c, 52);
R3(c, d, e, a, b, 53);
R3(b, c, d, e, a, 54);
R3(a, b, c, d, e, 55);
R3(e, a, b, c, d, 56);
R3(d, e, a, b, c, 57);
R3(c, d, e, a, b, 58);
R3(b, c, d, e, a, 59);
R4(a, b, c, d, e, 60);
R4(e, a, b, c, d, 61);
R4(d, e, a, b, c, 62);
R4(c, d, e, a, b, 63);
R4(b, c, d, e, a, 64);
R4(a, b, c, d, e, 65);
R4(e, a, b, c, d, 66);
R4(d, e, a, b, c, 67);
R4(c, d, e, a, b, 68);
R4(b, c, d, e, a, 69);
R4(a, b, c, d, e, 70);
R4(e, a, b, c, d, 71);
R4(d, e, a, b, c, 72);
R4(c, d, e, a, b, 73);
R4(b, c, d, e, a, 74);
R4(a, b, c, d, e, 75);
R4(e, a, b, c, d, 76);
R4(d, e, a, b, c, 77);
R4(c, d, e, a, b, 78);
R4(b, c, d, e, a, 79);
/* Add the working vars back into context.state[] */
state[0] += a;
state[1] += b;
state[2] += c;
state[3] += d;
state[4] += e;
#undef R4
#undef R3
#undef R2
#undef R1
#undef R0
#undef f4
#undef f3
#undef f2
#undef f1
#undef blk1
#undef blk0
}
/* Hash a single 512-bit block. This is the core of the algorithm. */
static void SHATransform(ULONG State[5], UCHAR Buffer[64])
{
ULONG a, b, c, d, e;
ULONG *Block;
Block = (ULONG*)Buffer;
/* Copy Context->State[] to working variables */
a = State[0];
b = State[1];
c = State[2];
d = State[3];
e = State[4];
/* 4 rounds of 20 operations each. Loop unrolled. */
R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3);
R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7);
R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11);
R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15);
R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23);
R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27);
R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31);
R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35);
R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39);
R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43);
R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47);
R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51);
R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55);
R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59);
R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63);
R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67);
R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71);
R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75);
R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79);
/* Add the working variables back into Context->State[] */
State[0] += a;
State[1] += b;
State[2] += c;
State[3] += d;
State[4] += e;
/* Wipe variables */
a = b = c = d = e = 0;
}
/*
* Hash a single 512-bit block. This is the core of the algorithm.
*/
void XSHA1_Transform(uint32_t state[5], const uint8_t buffer[64])
{
uint32_t a, b, c, d, e;
CHAR64LONG16 *block;
#ifdef SHA1HANDSOFF
CHAR64LONG16 workspace;
#endif
_DIAGASSERT(buffer != 0);
_DIAGASSERT(state != 0);
#ifdef SHA1HANDSOFF
block = &workspace;
(void)memcpy(block, buffer, 64);
#else
block = (CHAR64LONG16 *)(void *)buffer;
#endif
/* Copy context->state[] to working vars */
a = state[0];
b = state[1];
c = state[2];
d = state[3];
e = state[4];
#ifdef __sparc_v9__
do_R01(&a, &b, &c, &d, &e, block);
do_R2(&a, &b, &c, &d, &e, block);
do_R3(&a, &b, &c, &d, &e, block);
do_R4(&a, &b, &c, &d, &e, block);
#else
/* 4 rounds of 20 operations each. Loop unrolled. */
R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3);
R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7);
R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11);
R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15);
R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23);
R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27);
R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31);
R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35);
R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39);
R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43);
R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47);
R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51);
R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55);
R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59);
R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63);
R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67);
R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71);
R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75);
R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79);
#endif
/* Add the working vars back into context.state[] */
state[0] += a;
state[1] += b;
state[2] += c;
state[3] += d;
state[4] += e;
}
// hash a single 512-bit block. this is the core of the algorithm
static uint32_t sha1_transform(SHA1 *sha1, const uint8_t buffer[SHA1_BLOCK_LENGTH]) {
uint32_t a, b, c, d, e;
uint32_t block[SHA1_BLOCK_LENGTH / 4];
memcpy(&block, buffer, SHA1_BLOCK_LENGTH);
// copy sha1->state[] to working variables
a = sha1->state[0];
b = sha1->state[1];
c = sha1->state[2];
d = sha1->state[3];
e = sha1->state[4];
// 4 rounds of 20 operations each (loop unrolled)
R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3);
R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7);
R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11);
R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15);
R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23);
R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27);
R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31);
R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35);
R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39);
R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43);
R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47);
R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51);
R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55);
R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59);
R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63);
R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67);
R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71);
R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75);
R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79);
// add the working variables back into sha1->state[]
sha1->state[0] += a;
sha1->state[1] += b;
sha1->state[2] += c;
sha1->state[3] += d;
sha1->state[4] += e;
// wipe variables
a = b = c = d = e = 0;
return a; // return a to avoid dead-store warning from clang static analyzer
}
static void Transform(Sha* sha)
{
word32 W[SHA_BLOCK_SIZE / sizeof(word32)];
/* Copy context->state[] to working vars */
word32 a = sha->digest[0];
word32 b = sha->digest[1];
word32 c = sha->digest[2];
word32 d = sha->digest[3];
word32 e = sha->digest[4];
#ifdef USE_SLOW_SHA
word32 t, i;
for (i = 0; i < 16; i++) {
R0(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 20; i++) {
R1(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 40; i++) {
R2(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 60; i++) {
R3(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
for (; i < 80; i++) {
R4(a, b, c, d, e, i);
t = e; e = d; d = c; c = b; b = a; a = t;
}
#else
/* nearly 1 K bigger in code size but 25% faster */
/* 4 rounds of 20 operations each. Loop unrolled. */
R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3);
R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7);
R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11);
R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15);
R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23);
R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27);
R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31);
R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35);
R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39);
R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43);
R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47);
R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51);
R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55);
R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59);
R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63);
R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67);
R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71);
R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75);
R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79);
#endif
/* Add the working vars back into digest state[] */
sha->digest[0] += a;
sha->digest[1] += b;
sha->digest[2] += c;
sha->digest[3] += d;
sha->digest[4] += e;
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_tgt,
Matrix4 &transformation_matrix) const
{
transformation_matrix.setIdentity ();
// Assemble the correlation matrix H = source * target'
Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();
// rotated cloud
Eigen::Matrix<Scalar, 4, 4> R4;
R4.block (0, 0, 3, 3) = R;
R4 (0, 3) = 0;
R4 (1, 3) = 0;
R4 (2, 3) = 0;
R4 (3, 3) = 1;
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R4 * cloud_src_demean;
float scale1, scale2;
double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f;
for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx)
{
sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx);
sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx);
sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx);
sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx);
sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx);
sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx);
sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx);
sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx);
sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx);
}
scale1 = sqrt (sum_tt / sum_ss);
scale2 = sum_tt_ / sum_ss;
float scale = scale2;
transformation_matrix.topLeftCorner (3, 3) = scale * R;
const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc;
}