450 lines
15 KiB
C++
450 lines
15 KiB
C++
#include <memory.h>
|
|
#include <stdlib.h>
|
|
#include "HeadDetector.h"
|
|
#include "BottomFaceShapeExtraction.h"
|
|
|
|
//#define DEBUGDRAW
|
|
//#pragma optimize("",off)
|
|
|
|
|
|
typedef struct LineEvaluationResult_
|
|
{
|
|
u32 ulNumBerOfPixel;
|
|
s32 lSummOfValues;
|
|
s32 lSummOfSquareValues;
|
|
} LineEvaluationResult;
|
|
|
|
void PixelCompute(s16 *Pixel,LineEvaluationResult *pLE)
|
|
{
|
|
pLE->lSummOfValues += *Pixel;
|
|
pLE->lSummOfSquareValues += *Pixel * *Pixel;
|
|
pLE->ulNumBerOfPixel += 1;
|
|
}
|
|
/************************************************************/
|
|
/* DrawLine_Eval will compute the average color of the line */
|
|
/************************************************************/
|
|
void DrawLine_Eval(MAP16 *MapDST,float X1,float Y1,float X2,float Y2,LineEvaluationResult *pLE)
|
|
{
|
|
s32 DX,DY,Inc,Inc2,Counter;
|
|
s32 ADY,IThresh,ITCurrent,ITAdd;
|
|
s16 *B0;
|
|
s32 Pitch;
|
|
|
|
if (X1 < (s32)0) X1 = (s32)0;
|
|
if (Y1 < (s32)0) Y1 = (s32)0;
|
|
if (X1 > (s32)MapDST->SX-1) X1 = (s32)MapDST->SX-1;
|
|
if (Y1 > (s32)MapDST->SY-1) Y1 = (s32)MapDST->SY-1;
|
|
|
|
if (X2 < (s32)0) X2 = (s32)0;
|
|
if (Y2 < (s32)0) Y2 = (s32)0;
|
|
if (X2 > (s32)MapDST->SX-1) X2 = MapDST->SX-1;
|
|
if (Y2 > (s32)MapDST->SY-1) Y2 = MapDST->SY-1;
|
|
|
|
if (X2 < X1)
|
|
{
|
|
/* Swap */
|
|
DX = X1; X1 = X2; X2 = DX;
|
|
DY = Y1; Y1 = Y2; Y2 = DY;
|
|
}
|
|
|
|
|
|
B0 = MapDST->GetBase(); Pitch = MapDST->PITCH; B0 += (s32)Y1 * Pitch + (s32)X1;
|
|
PixelCompute(B0,pLE);
|
|
|
|
DX = X2 - X1; DY = Y2 - Y1;
|
|
if (DX == 0)
|
|
{
|
|
if (DY < 0)
|
|
{
|
|
DY = -DY;DY++;
|
|
while (DY--)
|
|
PixelCompute(B0 - DY * Pitch,pLE);
|
|
}
|
|
else
|
|
{
|
|
DY++;
|
|
while (DY--)
|
|
PixelCompute(B0 + DY * Pitch,pLE);
|
|
}
|
|
return;
|
|
}
|
|
if (DY == 0)
|
|
{
|
|
DX++;
|
|
while (DX--)
|
|
PixelCompute(B0++,pLE);
|
|
return;
|
|
}
|
|
ADY = DY;
|
|
if (ADY < 0) ADY = -ADY;
|
|
Inc2 = DY > 0 ? Pitch : -Pitch;
|
|
if (ADY > DX) {Inc = Inc2;Inc2 = 1;IThresh = ADY;ITAdd = DX;}
|
|
else {Inc = 1;IThresh = DX;ITAdd = ADY; };
|
|
ITCurrent = 0;
|
|
|
|
if (ADY > DX)
|
|
Counter = ADY;
|
|
else
|
|
Counter = DX;
|
|
|
|
Counter++;
|
|
while (Counter--)
|
|
{
|
|
PixelCompute(B0,pLE);
|
|
ITCurrent += ITAdd;
|
|
if (IThresh <= ITCurrent) {B0 += Inc2;ITCurrent -= IThresh;}
|
|
B0 += Inc;
|
|
}
|
|
}
|
|
|
|
/* Clip the line if it's out of the map */
|
|
void DrawLine_Clip_Eval(MAP16 *MapDST,float X1,float Y1,float X2,float Y2,LineEvaluationResult *pLE)
|
|
{
|
|
|
|
if ((X1 > (s32)MapDST->SX) && (X2 > (s32)MapDST->SX)) return;
|
|
if ((Y1 > (s32)MapDST->SY) && (Y2 > (s32)MapDST->SY)) return;
|
|
if ((X1 < 0) && (X2 < 0)) return;
|
|
if ((Y1 < 0) && (Y2 < 0)) return;
|
|
if ((X1 == X2) &&
|
|
(Y1 == Y2)) return;
|
|
|
|
for (u32 Counter = 0 ; Counter < 8 ; Counter++)
|
|
{
|
|
if (X1 < 0)
|
|
{
|
|
Y1 = ((float)Y1 + (float)(Y2 - Y1) * (float)X1 / (float)(X1 - X2));
|
|
X1 = 0;
|
|
}
|
|
if (X2 < 0)
|
|
{
|
|
Y2 = ((float)Y2 + (float)(Y1 - Y2) * (float)X2 / (float)(X2 - X1));
|
|
X2 = 0;
|
|
}
|
|
#define SWAP(a,b) {float SWP;SWP = a;a = b ; b = SWP; };
|
|
switch (Counter)
|
|
{
|
|
case 0:break;
|
|
case 1:X1 = MapDST->SX - X1;X2 = MapDST->SX - X2;break;
|
|
case 2:X1 = MapDST->SX - X1;X2 = MapDST->SX - X2;SWAP(X1,Y1);SWAP(X2,Y2);break;
|
|
case 3:X1 = MapDST->SY - X1;X2 = MapDST->SY - X2;break;
|
|
case 4:X1 = MapDST->SY - X1;X2 = MapDST->SY - X2;SWAP(X1,Y1);SWAP(X2,Y2);Counter = 80000;break;
|
|
}
|
|
}
|
|
|
|
DrawLine_Eval(MapDST,X1,Y1,X2,Y2,pLE);
|
|
}
|
|
|
|
void ComputeShape(MAP *pFaceTexture ,MAP16 *pCompute1, Vector *ControlPointsBase,u32 NumberOfVector,MAP *DrawDeMap,MATRIX *TMatrix,u32 Absolut)
|
|
{
|
|
#define MAX_VECTORS 20
|
|
static Vector ControlPointsBaseNormals[MAX_VECTORS /* Reach MAX */] ;
|
|
static Vector ControlPointsBoxMin[MAX_VECTORS /* Reach MAX */] ;
|
|
static Vector ControlPointsBoxMax[MAX_VECTORS /* Reach MAX */] ;
|
|
|
|
/* ***************************************************************************************************/
|
|
/* "Template" creation ******************************************************************************/
|
|
/* ***************************************************************************************************/
|
|
memset(ControlPointsBaseNormals , 0 , sizeof(ControlPointsBaseNormals));
|
|
|
|
|
|
for (u32 Counter = 0; Counter < NumberOfVector; Counter++)
|
|
{
|
|
Vector LocalStuff;
|
|
u32 OtherIndex = Counter+1;
|
|
if (Counter == NumberOfVector - 1 ) OtherIndex = Counter-1;
|
|
LocalStuff.x = -(ControlPointsBase[Counter] - ControlPointsBase[OtherIndex]).y;
|
|
LocalStuff.y = (ControlPointsBase[Counter] - ControlPointsBase[OtherIndex]).x;
|
|
if (Counter == NumberOfVector - 1 ) LocalStuff *= -1.0f;
|
|
LocalStuff.z = 0.0;
|
|
LocalStuff.Normalize();
|
|
ControlPointsBaseNormals[Counter] += LocalStuff;
|
|
ControlPointsBaseNormals[OtherIndex] += LocalStuff;
|
|
ControlPointsBaseNormals[Counter].Normalize();
|
|
ControlPointsBaseNormals[OtherIndex].Normalize();
|
|
}
|
|
|
|
LineEvaluationResult stLE;
|
|
|
|
|
|
for (u32 Counter = 0; Counter < NumberOfVector; Counter++)
|
|
{
|
|
ControlPointsBaseNormals[Counter] *= ControlPointsBase[Counter].z;
|
|
ControlPointsBoxMin[Counter] = ControlPointsBase[Counter] - ControlPointsBaseNormals[Counter];
|
|
ControlPointsBoxMax[Counter] = ControlPointsBase[Counter] + ControlPointsBaseNormals[Counter];
|
|
ControlPointsBoxMin[Counter].x *= pFaceTexture->SX;
|
|
ControlPointsBoxMin[Counter].y *= pFaceTexture->SY;
|
|
ControlPointsBoxMax[Counter].x *= pFaceTexture->SX;
|
|
ControlPointsBoxMax[Counter].y *= pFaceTexture->SY;
|
|
#ifdef DEBUGDRAW
|
|
|
|
DrawLine_Clip(pFaceTexture,(ControlPointsBoxMin[Counter]).x,(ControlPointsBoxMin[Counter]).y,(ControlPointsBoxMax[Counter]).x,(ControlPointsBoxMax[Counter]).y,0xff00);
|
|
#endif
|
|
|
|
}
|
|
|
|
#define Search_Disc 60 // discreetization along normals
|
|
#define SlopeMax 5 //
|
|
static float ErrorSpace[MAX_VECTORS /*Segments*/][Search_Disc][SlopeMax*2+1];
|
|
memset(ErrorSpace,0 ,sizeof(ErrorSpace));
|
|
/* ***************************************************************************************************/
|
|
/* Solver phase 1 compute all possible segment cost **************************************************/
|
|
/* ***************************************************************************************************/
|
|
for (u32 Counter = 0; Counter < NumberOfVector-1; Counter++)
|
|
{
|
|
float MaxValue = -410000000.0f;
|
|
Vector P1,P2;
|
|
for (int X = 0 ; X < Search_Disc ; X ++)
|
|
for (int Y = 0 ; Y < SlopeMax*2+1 ; Y ++)
|
|
{
|
|
if ((X + Y - SlopeMax >= 0) && (X + Y - SlopeMax < Search_Disc))
|
|
{
|
|
Vector PointA,PointB;
|
|
|
|
PointA = ControlPointsBoxMin[Counter] + ((ControlPointsBoxMax[Counter] - ControlPointsBoxMin[Counter]) * ((float)X / (float)(Search_Disc-1)));
|
|
PointB = ControlPointsBoxMin[Counter+1] + ((ControlPointsBoxMax[Counter+1] - ControlPointsBoxMin[Counter+1]) * ((float)(X+Y-SlopeMax) / (float)(Search_Disc-1)));
|
|
stLE.lSummOfValues = 0;
|
|
stLE.lSummOfSquareValues = 0;
|
|
stLE.ulNumBerOfPixel = 0;
|
|
/* Eval error along line */
|
|
DrawLine_Clip_Eval(pCompute1,PointA.x,PointA.y,PointB.x,PointB.y,&stLE);
|
|
float Result = 0;
|
|
float Result1 = 0;
|
|
float Result2 = 0;
|
|
if (stLE.ulNumBerOfPixel)
|
|
{
|
|
Result1 = (s32)((float)stLE.lSummOfValues / (float)stLE.ulNumBerOfPixel);
|
|
Result2 = (s32)((float)stLE.lSummOfSquareValues / (float)stLE.ulNumBerOfPixel);
|
|
Result = 0;//Result2 - Result1 * Result1;
|
|
Result += Result1;
|
|
}
|
|
/* stLE.lSummOfSquareValues *= 1.0f / (float)stLE.ulNumBerOfPixel;
|
|
float Result = (float)stLE.lSummOfValues - sqrt((float)stLE.lSummOfSquareValues - (float)stLE.lSummOfValues * (float)stLE.lSummOfValues);
|
|
|
|
/* Substract 2 parralelles lines */
|
|
/* Line 1 */
|
|
PointA += ControlPointsBaseNormals[Counter] * 5.0f;
|
|
PointB += ControlPointsBaseNormals[Counter+1] * 5.0f;
|
|
stLE.lSummOfValues = 0;
|
|
stLE.lSummOfSquareValues = 0;
|
|
stLE.ulNumBerOfPixel = 0;
|
|
// Eval error along line
|
|
DrawLine_Clip_Eval(pCompute1,PointA.x,PointA.y,PointB.x,PointB.y,&stLE);
|
|
if (stLE.ulNumBerOfPixel)
|
|
{
|
|
Result1 = (s32)((float)stLE.lSummOfValues / (float)stLE.ulNumBerOfPixel);
|
|
Result2 = (s32)((float)stLE.lSummOfSquareValues / (float)stLE.ulNumBerOfPixel);
|
|
Result -= 0;//Result2 - Result1 * Result1;
|
|
Result -= Result1;
|
|
}
|
|
|
|
|
|
if (Absolut && (Result < 0))
|
|
{
|
|
Result = -Result;
|
|
}
|
|
|
|
/* And store it in error space */
|
|
ErrorSpace[Counter /*Segments*/][X][Y] = (Result);
|
|
|
|
if (MaxValue < Result)
|
|
{
|
|
MaxValue = Result;
|
|
P1 = PointA;
|
|
P2 = PointB;
|
|
}
|
|
}
|
|
}
|
|
#ifdef DEBUGDRAW
|
|
DrawLine_Clip(pFaceTexture,P1.x,P1.y,P2.x,P2.y,0xff0000);
|
|
#endif
|
|
}
|
|
/* ***************************************************************************************************/
|
|
/* Solver phase 2 compute best path *****************************************************/
|
|
/* ***************************************************************************************************/
|
|
static s32 BestStack_MaxNextIndexes[MAX_VECTORS][Search_Disc];
|
|
static float BestStack_MaxSummedErrors[MAX_VECTORS][Search_Disc];
|
|
memset(BestStack_MaxNextIndexes,0,sizeof(BestStack_MaxNextIndexes));
|
|
memset(BestStack_MaxSummedErrors,0,sizeof(BestStack_MaxSummedErrors));
|
|
u32 Counter = NumberOfVector - 1;
|
|
while (Counter--)
|
|
{
|
|
for (int X = 0 ; X < Search_Disc ; X ++)
|
|
{
|
|
float MaxValue;
|
|
s32 MaxNextIndex;
|
|
MaxValue = -10000000.0f;
|
|
MaxNextIndex = 0;
|
|
for (int Y = 0 ; Y < SlopeMax*2+1 ; Y ++)
|
|
{
|
|
if ((X + Y - SlopeMax >= 0) && (X + Y - SlopeMax < Search_Disc))
|
|
{
|
|
if (ErrorSpace[Counter][X][Y] + BestStack_MaxSummedErrors[Counter+1][X + Y - SlopeMax] > MaxValue)
|
|
{
|
|
MaxValue = ErrorSpace[Counter][X][Y] + BestStack_MaxSummedErrors[Counter+1][X + Y - SlopeMax];
|
|
MaxNextIndex = X + Y - SlopeMax;
|
|
}
|
|
}
|
|
}
|
|
BestStack_MaxNextIndexes[Counter][X] = MaxNextIndex;
|
|
BestStack_MaxSummedErrors[Counter][X] = MaxValue;
|
|
}
|
|
}
|
|
/* Take best entry */
|
|
s32 MaxIndex = 0;
|
|
float MaxErrorStart = -10000000.0f;
|
|
for (Counter = 0; Counter < Search_Disc; Counter++)
|
|
{
|
|
if (BestStack_MaxSummedErrors[0][Counter] > MaxErrorStart)
|
|
{
|
|
MaxErrorStart = BestStack_MaxSummedErrors[0][Counter];
|
|
MaxIndex = Counter;
|
|
}
|
|
}
|
|
|
|
|
|
/* ***************************************************************************************************/
|
|
/* Solver phase 2 render result *****************************************************/
|
|
/* ***************************************************************************************************/
|
|
for (Counter = 0; Counter < NumberOfVector-1; )
|
|
{
|
|
Vector P1,P2;
|
|
|
|
P1 = ControlPointsBoxMin[Counter] + ((ControlPointsBoxMax[Counter] - ControlPointsBoxMin[Counter]) * ((float)MaxIndex / (float)(Search_Disc-1)));
|
|
MaxIndex = BestStack_MaxNextIndexes[Counter][MaxIndex];
|
|
Counter++;
|
|
P2 = ControlPointsBoxMin[Counter] + ((ControlPointsBoxMax[Counter] - ControlPointsBoxMin[Counter]) * ((float)MaxIndex / (float)(Search_Disc-1)));
|
|
#ifdef DEBUGDRAW
|
|
DrawLine_Clip(pFaceTexture,P1.x,P1.y,P2.x,P2.y,0xffFF00);
|
|
DrawLine_Clip(pFaceTexture,P1.x+1,P1.y,P2.x+1,P2.y,0xffFF00);
|
|
DrawLine_Clip(pFaceTexture,P1.x,P1.y+1,P2.x,P2.y+1,0xffFF00);
|
|
DrawLine_Clip(pFaceTexture,P1.x+1,P1.y+1,P2.x+1,P2.y+1,0xffFF00);
|
|
#endif
|
|
if (DrawDeMap)
|
|
{
|
|
if (TMatrix)
|
|
{
|
|
P1 = *TMatrix * P1;
|
|
P2 = *TMatrix * P2;
|
|
}
|
|
DrawLine_Clip(DrawDeMap,P1.x,P1.y,P2.x,P2.y,0xffFF00);
|
|
DrawLine_Clip(DrawDeMap,P1.x+1,P1.y,P2.x+1,P2.y,0xffFF00);
|
|
DrawLine_Clip(DrawDeMap,P1.x,P1.y+1,P2.x,P2.y+1,0xffFF00);
|
|
DrawLine_Clip(DrawDeMap,P1.x+1,P1.y+1,P2.x+1,P2.y+1,0xffFF00);//*/
|
|
}
|
|
}//*/
|
|
}
|
|
/* Bottom face template X,Y are screen coordinate betzeen 0 - 1, Z is the with of the search template*/
|
|
Vector ControlPoints_BOTTOM_FACE_SHAPE[] = {
|
|
Vector(0.83,0.6,0.08),
|
|
Vector(0.83,0.51,0.08),
|
|
Vector(0.82,0.43,0.08),
|
|
Vector(0.81,0.33,0.09),
|
|
Vector(0.75,0.25,0.11),
|
|
Vector(0.69,0.19,0.13),
|
|
Vector(0.62,0.15,0.14),
|
|
Vector(0.5,0.12,0.15),
|
|
Vector(1-0.62,0.15,0.14),
|
|
Vector(1-0.69,0.19,0.13),
|
|
Vector(1-0.75,0.25,0.11),
|
|
Vector(1-0.81,0.33,0.09),
|
|
Vector(1-0.85,0.43,0.08),
|
|
Vector(1-0.85,0.51,0.08),
|
|
Vector(1-0.85,0.6,0.08),
|
|
};
|
|
|
|
|
|
/* Eyebrown template */
|
|
Vector ControlPoints_EYE_SOURCIL[] = {
|
|
Vector(0.75,0.80,0.06),
|
|
Vector(0.70,0.80,0.06),
|
|
Vector(0.65,0.80,0.06),
|
|
Vector(0.60,0.80,0.06),
|
|
};
|
|
|
|
/* Eyebrown template */
|
|
Vector ControlPoints_mouth[] = {
|
|
Vector(0.70,0.35,0.0008),
|
|
Vector(0.65,0.35,0.08),
|
|
Vector(0.60,0.35,0.10),
|
|
Vector(0.50,0.35,0.12),
|
|
Vector(0.40,0.35,0.10),
|
|
Vector(0.35,0.35,0.08),
|
|
Vector(0.30,0.35,0.0008),
|
|
};
|
|
|
|
|
|
void ComputeBottomShape(MAP *pFaceTexture,MAP *pDebugDrawMap,DETECTEDFace *pFeatures,MATRIX *pTransform)
|
|
{
|
|
static MAP16 Compute1;
|
|
static u32 SizeAllocated = 0;
|
|
MATRIX Inverted;
|
|
Inverted.Identity();
|
|
if (pTransform) pTransform->Inverse(Inverted);
|
|
|
|
if ((SizeAllocated < pFaceTexture->SX*pFaceTexture->SY))
|
|
{
|
|
Compute1.SetSize(pFaceTexture->SX, pFaceTexture->SY);
|
|
SizeAllocated = pFaceTexture->SX * pFaceTexture->SY;
|
|
}
|
|
Compute1.SX = Compute1.PITCH = pFaceTexture->SX;
|
|
Compute1.SY = pFaceTexture->SY;
|
|
|
|
/* Gradient Map compute ******************************************************************************/
|
|
MAP_2_MAP16(pFaceTexture,&Compute1,18);
|
|
|
|
/* shape compute ******************************************************************************/
|
|
/* EYEBROWN SHAPE */
|
|
ComputeShape(pFaceTexture,&Compute1,ControlPoints_EYE_SOURCIL,sizeof(ControlPoints_EYE_SOURCIL)/sizeof(Vector),pDebugDrawMap,&Inverted);
|
|
u32 Counter = sizeof(ControlPoints_EYE_SOURCIL)/sizeof(Vector);
|
|
while (Counter--)
|
|
{
|
|
ControlPoints_EYE_SOURCIL[Counter].x = 1-ControlPoints_EYE_SOURCIL[Counter].x;
|
|
ControlPoints_EYE_SOURCIL[Counter].z = -ControlPoints_EYE_SOURCIL[Counter].z;
|
|
}
|
|
ComputeShape(pFaceTexture,&Compute1,ControlPoints_EYE_SOURCIL,sizeof(ControlPoints_EYE_SOURCIL)/sizeof(Vector),pDebugDrawMap,&Inverted);
|
|
Counter = sizeof(ControlPoints_EYE_SOURCIL)/sizeof(Vector);
|
|
while (Counter--)
|
|
{
|
|
ControlPoints_EYE_SOURCIL[Counter].x = 1-ControlPoints_EYE_SOURCIL[Counter].x;
|
|
ControlPoints_EYE_SOURCIL[Counter].z = -ControlPoints_EYE_SOURCIL[Counter].z;
|
|
}
|
|
|
|
/* Mouth shape ********************************************************************************/
|
|
if (pFeatures)
|
|
{
|
|
Vector MR,ML,MB,MI;
|
|
Vector Oo = Vector(1.0f /(float)pFaceTexture->SX, 1.0f /(float)pFaceTexture->SY,0);
|
|
MR = *pTransform * pFeatures->LeftMouse ;
|
|
ML = *pTransform * pFeatures->RightMouse ;
|
|
MR &= Oo;
|
|
ML &= Oo;
|
|
|
|
u32 NumBerOfMouthPoint = sizeof(ControlPoints_mouth)/sizeof(Vector);
|
|
MI = (MR - ML) * (1.0f / (float)(NumBerOfMouthPoint - 1));
|
|
MB = ML;
|
|
Counter = NumBerOfMouthPoint;
|
|
while (Counter--)
|
|
{
|
|
ControlPoints_mouth[Counter].x = MB.x;
|
|
ControlPoints_mouth[Counter].y = MB.y;
|
|
MB += MI;
|
|
}
|
|
ComputeShape(pFaceTexture,&Compute1,ControlPoints_mouth,sizeof(ControlPoints_mouth)/sizeof(Vector),pDebugDrawMap,&Inverted,0);
|
|
Counter = NumBerOfMouthPoint;
|
|
MB = MR;
|
|
while (Counter--)
|
|
{
|
|
ControlPoints_mouth[Counter].x = MB.x;
|
|
ControlPoints_mouth[Counter].y = MB.y;
|
|
MB -= MI;
|
|
}
|
|
ComputeShape(pFaceTexture,&Compute1,ControlPoints_mouth,sizeof(ControlPoints_mouth)/sizeof(Vector),pDebugDrawMap,&Inverted,0);
|
|
|
|
}
|
|
/* BOTTOM FACE SHAPE *************************************************************************************************/
|
|
ComputeShape(pFaceTexture,&Compute1,ControlPoints_BOTTOM_FACE_SHAPE,sizeof(ControlPoints_BOTTOM_FACE_SHAPE)/sizeof(Vector),pDebugDrawMap,&Inverted);
|
|
|
|
#ifdef DEBUGDRAW
|
|
MAP32_COPY(pFaceTexture,pDebugDrawMap);
|
|
#endif
|
|
}
|