JD2022-TU1/main/extern/Camcam/CCMap/MAP8.cpp

546 lines
12 KiB
C++

#include "windows.h"
#include "CCMap.h"
#include "math.h"
void ExtractPoint(MAP8 *MapDST,float X,float Y,float *Dest)
{
s32 BX,BY;
u8 *pBase;
BX = (s32)(X * 256.0f);
BY = (s32)(Y * 256.0f);
pBase = MapDST->GetBase() + (BX>>8) + (BY>>8) * MapDST->PITCH;
BX &= 255;
BY &= 255;
float VX1 = (float)*(pBase+0) * (float)(255 - BX) + (float)*(pBase+1) * (float)BX;
pBase += MapDST->PITCH;
float VX2 = (float)*(pBase+0) * (float)(255 - BX) + (float)*(pBase+1) * (float)BX;
*Dest = (VX1 * (float)(255 - BY) + VX2 * (float)BY) / 65536.0f;
}
float GetLineError(MAP8 *MapDST,float X1,float Y1,float X2,float Y2,float *Coefs,u32 Number)
{
float *pCoefs = (float *)Coefs;
float Ret = 0.0f;
X2 = X2 - X1;
Y2 = Y2 - Y1;
X2 /= (float)(Number - 1);
Y2 /= (float)(Number - 1);
X1 -= X2 * (float)(Number - 1) / 2.0f;
Y1 -= Y2 * (float)(Number - 1) / 2.0f;
/*
Eq = (Vi-VM)(Vi-VM) + (Vi-VM)(Vi-VM) + ....
Eq = Vi² + VM² - 2 * VM * Vi + ....
Eq = e{Vi²} + N*VM² - N * 2 * VM * Vi + ....
=> VM = e{Vi} / N
Eq = e{Vi²} + N*e{Vi}² / N² - (2 * (e{Vi} / N)) e{Vi}
Eq = e{Vi²} + e{Vi}² / N - 2 * e{Vi}² / N
Eq = e{Vi²} - e{Vi}² / N
Eq = e{Vi²} + e{Vi}² * 1/N² - e{Vi} * 2 * VM
Eq = e{Vi²} + e{Vi}² * 1/N² - e{Vi} * e{Vi} * 2/N
Eq = e{Vi²} + e{Vi}² * 1/N² - e{Vi}² * 2/N
Eq = e{Vi²} - e{Vi}² / N
Norm = 1.0f / sqrt(Eq);
VV = ((Vi-VM) * Coef[i] / Norm);
DP = (Vi0-VM)(Ci0) + (Vi0-VM)(Ci1) + ....
DP = Ci0 * Vi0 - Ci0 * VM + ...
DP = Ci0 * Vi0 + Ci1 * Vi1 - VM (e{Ci}) ...
e{Ci} = 0.0
DP = Ci0 * Vi0 + Ci1 * Vi1 + ...
*/
float DotProd = 0.0f;
float eVi = 0.0f;
float eViDP = 0.0f;
float eSqrVi = 0.0f;
while (Number --)
{
float C;
ExtractPoint(MapDST,X1,Y1,&C);
eVi += C;
eSqrVi += C * C;
eViDP += C * *pCoefs;
X1 += X2;
Y1 += Y2;
pCoefs++;
}
//VM = eVi / 6.0f;
//return eViDP ;
float Eq = eSqrVi - eVi * eVi / 8.0f;
return eViDP / sqrtf(Eq);
}
void SET_BORDER8(MAP8 *MapSRC,s32 Size,u8 Value)
{
u8 *pSrc,*pLst,*pLst2;
pSrc = MapSRC->GetBase();
pLst = MapSRC->GetBase() + Size * MapSRC->PITCH;
while (pSrc < pLst)
*(pSrc++) = Value;
pLst = MapSRC->GetBase() + (MapSRC->SY-Size) * MapSRC->PITCH;
while (pSrc < pLst)
{
pLst2 = pSrc + Size;
while (pSrc < pLst2)
*(pSrc++) = Value;
pSrc+= MapSRC->SX - Size*2;
pLst2 = pSrc + Size;
while (pSrc < pLst2)
*(pSrc++) = Value;
}
pLst = MapSRC->GetBase() + (MapSRC->SY) * MapSRC->PITCH;
while (pSrc < pLst)
*(pSrc++) = Value;
}
void MAP8_SQREType_H_Inplace(MAP8 *pSrc,u32 Rpo2)
{
// In place compute ET 8 bits horizontaly
u8 *PPd,*PPl,*PPdOfset;
u32 R = (1<<Rpo2 );
u32 INC = 0;
u32 INCSQR = 0;
PPd = pSrc->GetBase();
PPl = pSrc->GetBase() + pSrc->PITCH * pSrc->SY - R;
for (int i = 0 ; i < R ; i ++) {INC += (u32)PPd[i];INCSQR += (u32)PPd[i] * (u32)PPd[i];};
while (PPd < PPl)
{
u8 Lv = *PPd;
*PPd = (INCSQR - ((INC*INC)>>Rpo2))>>(Rpo2+8);
*PPd = sqrtf((float)((INCSQR - ((INC*INC)>>Rpo2))>>(Rpo2)));
INC -= (u32)Lv;
INCSQR -= (u32)Lv * (u32)Lv;
INC += *(PPd + R);
INCSQR += (u32)*(PPd + R) * (u32)*(PPd + R);
PPd ++;
}
PPd = pSrc->GetBase();
PPdOfset = PPl - (R>>1);
while (PPdOfset > PPd)
{
*(PPl--) = *(PPdOfset) + *(PPdOfset+1);
PPdOfset--;
}
}
void MAP8_SQREType_V_Inplace(MAP8 *pSrc,u32 Rpo2)
{
// In place blur 8 bits verticaly
u8 *PPd,*PPl,*PPdOfset;
u32 R = 1<<Rpo2;
u32 INC[1024];
u32 INCSQR[1024];
memset(INC,0,sizeof(INC));
memset(INCSQR,0,sizeof(INCSQR));
PPd = pSrc->GetBase();
for (int i = 0 ; i < pSrc->SX ; i ++)
{
for (int j = 0 ; j < R ; j ++)
{
u32 V = (u32)PPd[i+j *pSrc->PITCH];
INC[i] += V;
INCSQR[i] += V * V;
}
}
for (int i = 0 ; i < pSrc->SY-R ; i ++)
{
u32 *pINC,*pINCSQR;
u8 *PPd2;
pINC = INC;
pINCSQR = INCSQR;
PPd = pSrc->GetBase() + i * pSrc->PITCH;
PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
PPd2 = PPd + R * pSrc->PITCH;
while (PPd < PPl)
{
u32 Lv = *PPd;
*PPd = (*pINCSQR - ((*pINC**pINC)>>Rpo2))>>(Rpo2+8);
*PPd = sqrtf((float)((*pINCSQR - ((*pINC**pINC)>>Rpo2))>>(Rpo2)));
*pINC -= Lv;
*pINC += *(PPd2);
*pINCSQR -= Lv * Lv;
*pINCSQR += (u32)*(PPd2) * (u32)*(PPd2++);
pINC++;
pINCSQR++;
PPd ++;
}
}
for (int i = pSrc->SY-R; i < pSrc->SY ; i ++)
{
u32 *pINC;
pINC = INC;
PPd = pSrc->GetBase() + i * pSrc->PITCH;
PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
while (PPd < PPl)
{
u8 Lv = *PPd;
*PPd = (u8)(*pINC >>Rpo2 );
*pINC -= Lv;
pINC++;
PPd ++;
}
}
PPd = pSrc->GetBase();
PPdOfset = PPl - (R>>1) * pSrc->PITCH;
while (PPdOfset > PPd)
{
*(PPl--) = *(PPdOfset) + *(PPdOfset + pSrc->PITCH);
PPdOfset--;
}
}
void MAP8_BLUR8_H_Inplace(MAP8 *pSrc,u32 Rpo2)
{
// In place blur 8 bits horizontaly
u8 *PPd,*PPl,*PPdOfset;
u32 R = (1<<Rpo2 );
u32 INC = 0;
Rpo2++;
PPd = pSrc->GetBase();
PPl = pSrc->GetBase() + pSrc->PITCH * pSrc->SY - R;
for (int i = 0 ; i < R ; i ++) INC += (u32)PPd[i];
while (PPd < PPl)
{
u8 Lv = *PPd;
*PPd = (u8)(INC>>Rpo2);
INC -= Lv;
INC += *(PPd + R);
PPd ++;
}
PPd = pSrc->GetBase();
PPdOfset = PPl - (R>>1);
while (PPdOfset > PPd)
{
*(PPl--) = *(PPdOfset) + *(PPdOfset+1);
PPdOfset--;
}
}
void MAP8_BLUR8_V_Inplace(MAP8 *pSrc,u32 Rpo2)
{
// In place blur 8 bits verticaly
u8 *PPd,*PPl,*PPdOfset;
u32 R = 1<<Rpo2;
u32 INC[1024];
Rpo2 ++;
memset(INC,0,sizeof(INC));
PPd = pSrc->GetBase();
for (int i = 0 ; i < pSrc->SX ; i ++)
{
for (int j = 0 ; j < R ; j ++)
INC[i] += (u32)PPd[i+j *pSrc->PITCH];
}
for (int i = 0 ; i < pSrc->SY-R ; i ++)
{
u32 *pINC;
u8 *PPd2;
pINC = INC;
PPd = pSrc->GetBase() + i * pSrc->PITCH;
PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
PPd2 = PPd + R * pSrc->PITCH;
while (PPd < PPl)
{
u8 Lv = *PPd;
*PPd = (u8)(*pINC>>Rpo2);
*pINC -= Lv;
*pINC += *(PPd2++);
pINC++;
PPd ++;
}
}
for (int i = pSrc->SY-R; i < pSrc->SY ; i ++)
{
u32 *pINC;
pINC = INC;
PPd = pSrc->GetBase() + i * pSrc->PITCH;
PPl = pSrc->GetBase() + i * pSrc->PITCH + pSrc->SX;
while (PPd < PPl)
{
u8 Lv = *PPd;
*PPd = (u8)(*pINC >>Rpo2 );
*pINC -= Lv;
pINC++;
PPd ++;
}
}
PPd = pSrc->GetBase();
PPdOfset = PPl - (R>>1) * pSrc->PITCH;
while (PPdOfset > PPd)
{
*(PPl--) = *(PPdOfset) + *(PPdOfset + pSrc->PITCH);
PPdOfset--;
}
}
void MAP8_WAVELET_H_Inplace(MAP8 *pSrc)
{
u8 *PPSrc,*PPSrcppitch,*PPDst,*PPl,*PPdOfset;
u32 Counter;
Counter = pSrc->SY;
PPDst = pSrc->GetBase();
while (Counter--)
{
PPSrc = PPDst;
PPl = PPSrc+pSrc->SX;
while (PPSrc < PPl)
{
*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc+1))>>1);
PPDst++;
PPSrc+=2;
}
PPSrc = PPl-pSrc->SX;
while (PPSrc < PPl)
{
*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc+1))>>1);
PPDst++;
PPSrc+=2;
}
PPDst+=pSrc->PITCH - pSrc->SX;
}
}
void MAP8_WAVELET_V_Inplace(MAP8 *pSrc)
{
u8 *PPSrc,*PPDst,*PPl,*PPdOfset;
u32 Counter;
PPDst = pSrc->GetBase();
PPSrc = PPDst;
Counter = pSrc->SY>>1;
while (Counter--)
{
PPl = PPDst+pSrc->SX;
while (PPDst < PPl)
{
*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc + pSrc->PITCH))>>1);
PPDst++;
PPSrc++;
}
PPDst+=pSrc->PITCH - pSrc->SX;
PPSrc+=pSrc->PITCH*2 - pSrc->SX;
}
Counter = pSrc->SY>>1;
PPSrc = pSrc->GetBase();
while (Counter--)
{
PPl = PPDst+pSrc->SX;
while (PPDst < PPl)
{
*PPDst = (u8)(((u32)*PPSrc + (u32)*(PPSrc + pSrc->PITCH))>>1);
PPDst++;
PPSrc++;
}
PPDst+=pSrc->PITCH - pSrc->SX;
PPSrc+=pSrc->PITCH*2 - pSrc->SX;
}
}
//#pragma optimize("",off)
#define U8_SWAP(a,b) {u8 Swp;Swp = a;a = b ; b = Swp; }
void MAP8_ShiftLine(u8 *pSrc,s32 Shift,s32 Lenght)
{
u8 CpyL[2048];
// In place shift 8 bits horizontaly
if (Shift < 0) Shift = Lenght + Shift;
if (Shift < 0) return;
if (Shift >= Lenght) Shift = Lenght -1;
memcpy(CpyL,pSrc,Lenght);
memcpy(pSrc,CpyL+Lenght-Shift,Shift);
memcpy(pSrc+Shift,CpyL,Lenght-Shift);
}
void MAP8_BLUR8_ScissorX_Inplace(MAP8 *pSrc,s32 FactorX)
{
u8 *pCrnt,*pLast;
float Inc,Base;
pCrnt = pSrc->GetBase();
pLast = pCrnt + pSrc->PITCH * pSrc->SY;
Base = 0.0f;
Inc = (float)FactorX / (float)pSrc->SY;
while (pCrnt < pLast)
{
MAP8_ShiftLine(pCrnt,(s32)Base,pSrc->SX);
Base+=Inc;
pCrnt+=pSrc->PITCH;
}
}
void MAP8_BLUR8_CopyTranspose(MAP8 *pSrc,MAP8 *pDst)
{
u8 *pucS,*pucD,*pucL,*pucLL;
u32 HL;
pDst->PITCH = pDst->SX = pSrc->SY;
pDst->SY = pSrc->SX;
pucS = pSrc->GetBase();
pucD = pDst->GetBase();
HL = pSrc->PITCH * pSrc->SY;
pucLL = pucS + pSrc->SX;
while (pucS < pucLL)
{
pucL = pucS + HL;
while (pucS < pucL)
{
*(pucD++) = *pucS;
pucS+=pSrc->PITCH;
}
pucS -= HL - 1;
}
}
void MAP8_BLUR_XY(MAP8 *pSrc,MAP8 *pTmp,u8 Alpha, u32 Rpo2 , u32 Iteration)
{
SET_BORDER8(pSrc,1<<(Rpo2-1),0);
if ((Alpha & 63) == 0) // Is a quadrant
{
if (Alpha & 64)
while (Iteration--) MAP8_BLUR8_H_Inplace(pSrc,Rpo2); // 64 or 192
else
while (Iteration--) MAP8_BLUR8_V_Inplace(pSrc,Rpo2); // 0 or 128
return;
}
if ( Alpha > 128) Alpha -= 128;
u32 Xalpha = ((u32)Alpha) + 32;
if (Xalpha & 64)
{
s32 FactorX = (float)pSrc->SY * tanf((64.0f + (float)Alpha) * 2.0f * 3.1415927f / 256.0f );
MAP8_BLUR8_CopyTranspose(pSrc,pTmp);
MAP8_BLUR8_ScissorX_Inplace(pTmp,-FactorX);
while (Iteration--) MAP8_BLUR8_V_Inplace(pTmp,Rpo2);
MAP8_BLUR8_ScissorX_Inplace(pTmp,FactorX);
MAP8_BLUR8_CopyTranspose(pTmp,pSrc);
} else
{
s32 FactorX = (float)pSrc->SY * tanf((float)Alpha * 2.0f * 3.1415927f / 256.0f );
MAP8_BLUR8_ScissorX_Inplace(pSrc,FactorX);
while (Iteration--) MAP8_BLUR8_V_Inplace(pSrc,Rpo2);
MAP8_BLUR8_ScissorX_Inplace(pSrc,-FactorX);
}
}
void MAP8_BLUR8_Inplace(MAP8 *pSrc, u32 Rpo2 , u32 Iteration)
{
SET_BORDER8(pSrc,1<<(Rpo2-1),0);
while (Iteration--)
{
MAP8_BLUR8_V_Inplace(pSrc,Rpo2);
MAP8_BLUR8_H_Inplace(pSrc,Rpo2);
}
}
void MAP8_COMPUTEO2(MAP8 *pSrc,MAP8 *pDst)
{
*pDst = *pSrc;
pDst->MoveBase(pSrc->SY*pSrc->PITCH) ;
pDst->SX>>=1;
pDst->SY>>=1;
pDst->PITCH>>=1;
u32 LineCounter = pDst->SY & (~1);
while (LineCounter--)
{
u8 *PixSrc,*PixLast,*PixDest;
PixDest = pDst->GetBase()+LineCounter*pDst->PITCH;
PixSrc = pSrc->GetBase()+(LineCounter*2)*pSrc->PITCH;
PixLast = PixSrc+pSrc->SX;
while (PixSrc<PixLast)
{
*PixDest = (u8)((((u32)*PixSrc) + ((u32)*(PixSrc+1)) + ((u32)*(PixSrc+pSrc->PITCH)) + ((u32)*(PixSrc+pSrc->PITCH+1)))>>2);
PixSrc+=2;
PixDest++;
}
}
}
void MAP_2_MAP8(/*IN*/MAP *pSrc,/*OUT*/MAP8 *pDst,/*IN*/u8 *pTempBuffer)
{
if (pTempBuffer) pDst->SetBase(pTempBuffer);
pDst->PITCH = pDst->SX = pSrc->SX;
pDst->SY = pSrc->SY;
s32 *PPs;
u8 *PPd,*PPl,*PPll,*PPDeriv;
/* 1 Compute 8b */
PPs = pSrc->GetBase();
PPd = pDst->GetBase();
PPl = pDst->GetBase() + pDst->SX * pDst->SY;
while (PPd < PPl)
{
PPll = PPd + pDst->SX;
while (PPd < PPll)
{
*PPd = (u8)(*PPs>>8);
PPd ++;
PPs ++;
}
PPs += pSrc->PITCH - pSrc->SX;
}
}
void MAP8_2_MAP(/*IN*/MAP8 *pSrc,MAP *pDst)
{
u8 *PPs;
s32 *PPd,*PPl,*PPll,*PPDeriv;
/* 1 Compute 8b */
PPs = pSrc->GetBase();
PPd = pDst->GetBase();
PPl = pDst->GetBase() + pDst->PITCH * pDst->SY;
while (PPd < PPl)
{
PPll = PPd + pDst->SX;
while (PPd < PPll)
{
*PPd = (s32)(*PPs);
*PPd |= *PPd<<8;
*PPd |= *PPd<<8;
PPd ++;
PPs ++;
}
PPs += pSrc->PITCH - pSrc->SX;
PPd += pDst->PITCH - pDst->SX;
}
}
void MAP8_2_MAP_DIFF(/*IN*/MAP8 *pSrc,MAP *pDst)
{
u8 *PPs;
s32 *PPd,*PPl,*PPll,*PPDeriv;
/* 1 Compute 8b */
PPs = pSrc->GetBase();
PPd = pDst->GetBase();
PPl = pDst->GetBase() + pDst->PITCH * pDst->SY;
while (PPd < PPl)
{
PPll = PPd + pDst->SX;
while (PPd < PPll)
{
s32 V;
V = ((((*PPd>>8)&0xff) - (s32)(*PPs) )>>1) + 128;
//if (V > 128) V ^= 255;
*PPd = V;
*PPd |= *PPd<<8;
*PPd |= *PPd<<8;
PPd ++;
PPs ++;
}
PPs += pSrc->PITCH - pSrc->SX;
PPd += pDst->PITCH - pDst->SX;
}
}