FVector UBlendSpaceBase::FilterInput(FBlendFilter * Filter, const FVector& BlendInput, float DeltaTime) const
{
if (bNeedReinitializeFilter)
{
InitializeFilter(Filter);
bNeedReinitializeFilter = false;
}
FVector FilteredBlendInput;
FilteredBlendInput.X = Filter->FilterPerAxis[0].GetFilteredData(BlendInput.X, DeltaTime);
FilteredBlendInput.Y = Filter->FilterPerAxis[1].GetFilteredData(BlendInput.Y, DeltaTime);
FilteredBlendInput.Z = Filter->FilterPerAxis[2].GetFilteredData(BlendInput.Z, DeltaTime);
return FilteredBlendInput;
}
TSharedPtr<const FNavigationQueryFilter> UNavigationQueryFilter::GetQueryFilter(const ANavigationData* NavData) const
{
TSharedPtr<const FNavigationQueryFilter> SharedFilter = NavData->GetQueryFilter(GetClass());
if (!SharedFilter.IsValid())
{
FNavigationQueryFilter* NavFilter = new FNavigationQueryFilter();
NavFilter->SetFilterImplementation(NavData->GetDefaultQueryFilterImpl());
InitializeFilter(NavData, NavFilter);
SharedFilter = MakeShareable(NavFilter);
((ANavigationData*)NavData)->StoreQueryFilter(GetClass(), SharedFilter);
}
return SharedFilter;
}
void TrackBall2D(int xTrackCenter, int yTrackCenter, int trackRange, Filter *ptr_oldFilterGreen, Filter *ptr_oldFilterBlue, unsigned short ary2_imgFrame[XLCD][YLCD], float ary2_rgb2hsvTable[NUM_RGB][3], short ary2_rgb2labTable[NUM_RGB][3]){
int i, j, xFrom, yFrom, xTo, yTo, hueFlagGreen = 0, hueFlagBlue=0;
float hTemp, hLGreen, hUGreen, sTemp, vTemp, hLBlue, hUBlue;
int L,a,b;
int labNumber = 0;
float avgL = 0, avgA = 0, avgB = 0, stdL = 0, stdA = 0, stdB = 0;
//int rTemp, gTemp, bTemp;
Filter newComerGreen, newComerBlue;
InitializeFilter(ptr_oldFilterGreen->ballColor, &newComerGreen);
InitializeFilter(ptr_oldFilterBlue->ballColor, &newComerBlue);
//newComer.avgL = ptr_oldFilter->avgL;
//newComer.avgA = ptr_oldFilter->avgA;
//newComer.avgB = ptr_oldFilter->avgB;
//newComer.stdL = ptr_oldFilter->stdL;
//newComer.stdA = ptr_oldFilter->stdA;
//newComer.stdB = ptr_oldFilter->stdB;
//xFrom = ptr_oldFilter->xFrom;
//xTo = ptr_oldFilter->xTo;
//yFrom = ptr_oldFilter->yFrom;
//yTo = ptr_oldFilter->yTo;
xFrom = Min(XLCD, Max(0, xTrackCenter-trackRange));
xTo = Min(XLCD, Max(0, xTrackCenter+trackRange));
yFrom = Min(YLCD, Max(0, yTrackCenter-trackRange));
yTo = Min(YLCD, Max(0, yTrackCenter+trackRange));
//Sweep given image matrix for the color ball
for(j=xFrom;j<xTo;j++){
for(i=yFrom;i<yTo;i++){
//hTemp = RGB2HSV(ary2_imgFrame[j][i]);
hTemp = ary2_rgb2hsvTable[ary2_imgFrame[j][i]][0];
sTemp = ary2_rgb2hsvTable[ary2_imgFrame[j][i]][1];
vTemp = ary2_rgb2hsvTable[ary2_imgFrame[j][i]][2];
//rTemp = (int)((((ary2_imgFrame[j][i] & 0xf800) >> 11) * 255 / 31));
//gTemp = (int)((((ary2_imgFrame[j][i] & 0x07e0) >> 5) * 255 / 63));
//bTemp = (int)(((ary2_imgFrame[j][i] & 0x001f) * 255 / 31));
hLGreen = ptr_oldFilterGreen->hThreshold-ptr_oldFilterGreen->hBias;
hUGreen = ptr_oldFilterGreen->hThreshold+ptr_oldFilterGreen->hBias;
hueFlagGreen = hTemp > hLGreen && hTemp < hUGreen && sTemp > 144 && vTemp > 102;
hLBlue = ptr_oldFilterBlue->hThreshold-ptr_oldFilterBlue->hBias;
hUBlue = ptr_oldFilterBlue->hThreshold+ptr_oldFilterBlue->hBias;
hueFlagBlue = hTemp > hLBlue && hTemp < hUBlue && sTemp > 144 && vTemp > 102;
if(hueFlagGreen){
hueFlagGreen = 0;
newComerGreen.ballSize++;
newComerGreen.xCenter += j;
newComerGreen.yCenter += i;
}else if(hueFlagBlue){
hueFlagBlue = 0;
newComerBlue.ballSize++;
newComerBlue.xCenter += j;
newComerBlue.yCenter += i;
}else{
hueFlagGreen = 0;
hueFlagBlue = 0;
labNumber++;
L = ary2_rgb2labTable[ary2_imgFrame[j][i]][0];
a = ary2_rgb2labTable[ary2_imgFrame[j][i]][1];
b = ary2_rgb2labTable[ary2_imgFrame[j][i]][2];
avgL += L;
avgA += a;
avgB += b;
stdL += L * L;
stdA += a * a;
stdB += b * b;
}
/*if(hL < 0){
hSwap = hU;
hU = hL + 360;
hL = hSwap;
}
if(hU >= 360){
hSwap = hL;
hL = hU - 360;
hU = hSwap;
}*/
/*if(ptr_oldFilter->ballColor) //Blue
//hueFlag = hTemp>hL && hTemp<hU;
//hueFlag = hTemp>hL && hTemp<hU && rTemp > 50 && rTemp < 90 && gTemp > 95 && gTemp < 130 && bTemp > 130 && bTemp < 180;
hueFlag = hTemp > hL && hTemp < hU && sTemp > 144 && vTemp > 102;
else //Red
//hueFlag = hTemp<hL || hTemp>hU && rTemp > 190 && rTemp < 225 && gTemp > 100 && gTemp < 150 && bTemp > 60 && bTemp < 120;
hueFlag = (hTemp < hL || hTemp > hU) && sTemp > 144 && vTemp > 102;*/
/*if(hueFlag){
hueFlag = 0;
newComer.ballSize++;
newComer.xCenter += j;
newComer.yCenter += i;
}else{
labNumber++;
L = ary2_rgb2labTable[ary2_imgFrame[j][i]][0];
a = ary2_rgb2labTable[ary2_imgFrame[j][i]][1];
b = ary2_rgb2labTable[ary2_imgFrame[j][i]][2];
avgL += L;
avgA += a;
avgB += b;
stdL += L * L;
stdA += a * a;
stdB += b * b;
}*/
}
}
switch((newComerGreen.ballSize > newComerGreen.lowerBound) * 2 + (newComerBlue.ballSize > newComerBlue.lowerBound)){
case 0:
InitializeFilter(newComerGreen.ballColor, ptr_oldFilterGreen);
InitializeFilter(newComerBlue.ballColor, ptr_oldFilterBlue);
backgroundAvgL = avgL/labNumber;
backgroundAvgA = avgA/labNumber;
backgroundAvgB = avgB/labNumber;
backgroundStdL = sqrt(stdL/labNumber - backgroundAvgL * backgroundAvgL);
backgroundStdA = sqrt(stdA/labNumber - backgroundAvgA * backgroundAvgA);
backgroundStdB = sqrt(stdB/labNumber - backgroundAvgB * backgroundAvgB);
/*Adjust*/
decideLAB_apply = 0;
/*Adjust*/
break;
case 1:
newComerBlue.ballFound = 1;
newComerBlue.xCenter = newComerBlue.xCenter/newComerBlue.ballSize;
newComerBlue.yCenter = newComerBlue.yCenter/newComerBlue.ballSize;
if(pow(newComerBlue.xCenter - ptr_oldFilterBlue->xCenter,2) + pow(newComerBlue.yCenter - ptr_oldFilterBlue->yCenter,2) < displacementThreshold){
newComerBlue.xCenter = ptr_oldFilterBlue->xCenter;
newComerBlue.yCenter = ptr_oldFilterBlue->yCenter;
}
*ptr_oldFilterBlue = newComerBlue;
/*Adjust*/
decideLAB_apply++;
/*Adjust*/
break;
case 2:
newComerGreen.ballFound = 1;
newComerGreen.xCenter = newComerGreen.xCenter/newComerGreen.ballSize;
newComerGreen.yCenter = newComerGreen.yCenter/newComerGreen.ballSize;
if(pow(newComerGreen.xCenter - ptr_oldFilterGreen->xCenter,2) + pow(newComerGreen.yCenter - ptr_oldFilterGreen->yCenter,2) < displacementThreshold){
newComerGreen.xCenter = ptr_oldFilterGreen->xCenter;
newComerGreen.yCenter = ptr_oldFilterGreen->yCenter;
}
*ptr_oldFilterGreen = newComerGreen;
/*Adjust*/
decideLAB_apply++;
/*Adjust*/
break;
case 3:
newComerBlue.ballFound = 1;
newComerBlue.xCenter = newComerBlue.xCenter/newComerBlue.ballSize;
newComerBlue.yCenter = newComerBlue.yCenter/newComerBlue.ballSize;
if(pow(newComerBlue.xCenter - ptr_oldFilterBlue->xCenter,2) + pow(newComerBlue.yCenter - ptr_oldFilterBlue->yCenter,2) < displacementThreshold){
newComerBlue.xCenter = ptr_oldFilterBlue->xCenter;
newComerBlue.yCenter = ptr_oldFilterBlue->yCenter;
}
*ptr_oldFilterBlue = newComerBlue;
newComerGreen.ballFound = 1;
newComerGreen.xCenter = newComerGreen.xCenter/newComerGreen.ballSize;
newComerGreen.yCenter = newComerGreen.yCenter/newComerGreen.ballSize;
if(pow(newComerGreen.xCenter - ptr_oldFilterGreen->xCenter,2) + pow(newComerGreen.yCenter - ptr_oldFilterGreen->yCenter,2) < displacementThreshold){
newComerGreen.xCenter = ptr_oldFilterGreen->xCenter;
newComerGreen.yCenter = ptr_oldFilterGreen->yCenter;
}
*ptr_oldFilterGreen = newComerGreen;
/*Adjust*/
decideLAB_apply++;
/*Adjust*/
break;
}
//newComer.xFrom = Min(XLCD, Max(0, floor(ptr_oldFilter->xFrom + newComer.xCenter - ptr_oldFilter->xCenter)));
//newComer.xTo = Min(XLCD, Max(0, floor(ptr_oldFilter->xTo + newComer.xCenter - ptr_oldFilter->xCenter)));
//newComer.yFrom = Min(YLCD, Max(0, floor(ptr_oldFilter->yFrom + newComer.yCenter - ptr_oldFilter->yCenter)));
//newComer.yTo = Min(YLCD, Max(0, floor(ptr_oldFilter->yTo + newComer.yCenter - ptr_oldFilter->yCenter)));
//newComer.xFrom = Min(XLCD, Max(0, floor(newComer.xCenter- sqrt(newComer.ballSize) / 2 - newComer.boxPadding)));
//newComer.xTo = Min(XLCD, Max(0, floor(newComer.xFrom + sqrt(newComer.ballSize) + 2 * newComer.boxPadding)));
//newComer.yFrom = Min(XLCD, Max(0, floor(newComer.yCenter- sqrt(newComer.ballSize) /2 - newComer.boxPadding)));
//newComer.yTo = Min(XLCD, Max(0, floor(newComer.yFrom + sqrt(newComer.ballSize) + 2 * newComer.boxPadding)));
//newComer.xFrom = Min(XLCD, Max(0, floor(newComer.xCenter-100)));
//newComer.xTo = Min(XLCD, Max(0, floor(newComer.xFrom + 200)));
//newComer.yFrom = Min(XLCD, Max(0, floor(newComer.yCenter-100)));
//newComer.yTo = Min(XLCD, Max(0, floor(newComer.yFrom + 200)));
//newComer.xFrom = Min(XLCD, Max(0, floor(ptr_oldFilter->xFrom)));
//newComer.xTo = Min(XLCD, Max(0, floor(ptr_oldFilter->xTo)));
//newComer.yFrom = Min(YLCD, Max(0, floor(ptr_oldFilter->yFrom)));
//newComer.yTo = Min(YLCD, Max(0, floor(ptr_oldFilter->yTo)));
/*}else{
InitializeFilter(newComer.ballColor, ptr_oldFilter);
ptr_oldFilter->avgL = avgL/labNumber;
ptr_oldFilter->avgA = avgA/labNumber;
ptr_oldFilter->avgB = avgB/labNumber;
ptr_oldFilter->stdL = sqrt(stdL/labNumber - ptr_oldFilter->avgL * ptr_oldFilter->avgL);
ptr_oldFilter->stdA = sqrt(stdA/labNumber - ptr_oldFilter->avgA * ptr_oldFilter->avgA);
ptr_oldFilter->stdB = sqrt(stdB/labNumber - ptr_oldFilter->avgB * ptr_oldFilter->avgB);
}*/
}
