Vec3q SATSampler::operator()(const Vec2q &uv,const Vec2q &diff) const {
f32x4b fullMask=diff.x>=0.5f||diff.x>= 0.5f;
if(ForAll(fullMask)) return Vec3q(avg.x,avg.y,avg.z);
Vec2q tDiff=diff*floatq(0.5f);
Vec2q a=(uv-tDiff),b=(uv+tDiff);
a*=Vec2q(floatq(w),floatq(h));
b*=Vec2q(floatq(w),floatq(h));
i32x4 ax(a.x),ay(a.y);
i32x4 bx(b.x),by(b.y);
ax&=wMask; ay&=hMask;
bx&=wMask; by&=hMask;
union { __m128 count; float countf[4]; };
TSample sum[4];
i32x4 one(1);
if(ForAll(ax<=bx&&ay<=by)) {
count = (f32x4(by-ay+one)*f32x4(bx-ax+one)).m;
ComputeRect(ax,ay,bx,by,sum);
}
else for(int k=0;k<4;k++) {
if(ax[k]>bx[k]) {
if(ay[k]>by[k]) {
countf[k]=(bx[k]+1)*(by[k]+1)+(w-ax[k])*(h-ay[k]);
sum[k]=ComputeRect(0,0,bx[k],by[k])+ComputeRect(ax[k],ay[k],w-1,h-1);
}
else {
countf[k]=(bx[k]+1+w-ax[k])*(by[k]-ay[k]+1);
sum[k]=ComputeRect(0,ay[k],bx[k],by[k])+ComputeRect(ax[k],ay[k],w-1,by[k]);
}
}
else {
if(ay[k]>by[k]) {
countf[k]=(bx[k]-ax[k]+1)*(by[k]+h+1-ay[k]);
sum[k]=ComputeRect(ax[k],0,bx[k],by[k])+ComputeRect(ax[k],ay[k],bx[k],h-1);
}
else {
countf[k]=(by[k]-ay[k]+1)*(bx[k]-ax[k]+1);
sum[k]=ComputeRect(ax[k],ay[k],bx[k],by[k]);
}
}
}
union {
__m128 out[3];
struct { float ox[4]; float oy[4]; float oz[4]; } o;
};
o.ox[0]=sum[0].R(); o.oy[0]=sum[0].G(); o.oz[0]=sum[0].B();
o.ox[1]=sum[1].R(); o.oy[1]=sum[1].G(); o.oz[1]=sum[1].B();
o.ox[2]=sum[2].R(); o.oy[2]=sum[2].G(); o.oz[2]=sum[2].B();
o.ox[3]=sum[3].R(); o.oy[3]=sum[3].G(); o.oz[3]=sum[3].B();
return Condition(fullMask,Vec3q(avg.x,avg.y,avg.z),
Vec3q(out[0], out[1], out[2]) * Inv(floatq(count) * 255.0f));
}
TEST(RelativePositionSummerTest, TwoDirectionsOneDim)
{
auto positions = std::list<pos> { {1,1} };
List list_ref {positions};
auto command = Command::TestAdvancedTermCollector<1>{};
auto summer = Manipulator::RelPosSummer<1>{&command};
summer.ForAll(list_ref.begin(), list_ref.end());
auto result = command.ExtractData();
auto expected = Command::TestAdvancedTermCollector<1>::container_type{};
expected.insert( { { {{2}} }, 1} );
EXPECT_PRED_FORMAT2(UnitTest::ContainerCompare, expected, result);
}
TEST(RelativePositionSummerTest, TwoTermsThreeDims)
{
auto positions = std::list<pos> { {1}, {2} };
List list_ref {positions};
auto command = Command::TestSimpleTermCollector<3>{};
auto summer = Manipulator::RelPosSummer<3>{&command};
summer.ForAll(list_ref.begin(), list_ref.end());
auto result = command.ExtractData();
auto expected_result = Command::TestSimpleTermCollector<3>::container_type {
{ {{1,0,0}}, {{2,0,0}} }, { {{0,1,0}}, {{0,2,0}} }, { {{0,0,1}}, {{0,0,2}} } };
EXPECT_EQ(expected_result, result);
}
