MMatrix MotionSyn::synTrainsiton(const std::size_t identity, const std::size_t content1,const std::size_t content2,
const std::size_t length, CVector3D<double> initPos, double &curState)
{
std::vector<MMatrix> xStar(3);
xStar[0].resize(length,2);
xStar[1].resize(length, mFactors[1]->sizeCol());
xStar[2].resize(length, mFactors[2]->sizeCol());
MMatrix actor = mFactors[1]->subMMatrix(identity,0,1,mFactors[1]->sizeCol());
/*std::vector<double> steps;
double total = 0.0;
for (std::size_t i = 0; i < length; i++)
{
MMatrix newContent(1,mFactors[2]->sizeCol());
for(std::size_t t = 0; t < newContent.sizeCol(); t++)
{
double val = (1 - double(t)/length) * mFactors[2]->get(content1,t)
+ double(t)/length * mFactors[2]->get(content2,t);
newContent.assign(val,t);
}
MMatrix kron = actor.kron(newContent);
MMatrix val = mGPM->predict(kron);
total += val.get(0);
steps.push_back(val.get(0));
xStar[1].copyRowRow(i,*mFactors[1],identity);
xStar[2].copyRowRow(i,newContent,0);
}
total = 6.28 - total;
for (std::size_t i = 0; i < length; i++)
{
total += steps[i];
xStar[0].assign(cos(6.28-steps[i]*(length-i)), i, 0);
xStar[0].assign(sin(6.28-steps[i]*(length-i)), i, 1);
}*/
for (std::size_t i = 0; i < length; i++)
{
if(i < 50)
{
MMatrix newContent(1,mFactors[2]->sizeCol());
for(std::size_t t = 0; t < newContent.sizeCol(); t++)
{
double val = (1 - double(t)/50) * mFactors[2]->get(content1,t)
+ double(t)/50 * mFactors[2]->get(content2,t);
newContent.assign(val,t);
}
MMatrix kron = actor.kron(newContent);
MMatrix val = mGPM->predict(kron);
double step = val.get(0);
curState += step;
xStar[2].copyRowRow(i,newContent,0);
/* MMatrix content = mFactors[2]->subMMatrix(content1,0,1,mFactors[2]->sizeCol());
MMatrix kron = actor.kron(content);
MMatrix val = mGPM->predict(kron);
double step = val.get(0);
curState += step;
xStar[2].copyRowRow(i,content,0);*/
}
else
{
MMatrix content = mFactors[2]->subMMatrix(content2,0,1,mFactors[2]->sizeCol());
MMatrix kron = actor.kron(content);
MMatrix val = mGPM->predict(kron);
double step = val.get(0);
curState += step;
xStar[2].copyRowRow(i,content,0);
}
xStar[0].assign(cos(curState), i, 0);
xStar[0].assign(sin(curState), i, 1);
xStar[1].copyRowRow(i,*mFactors[1],identity);
}
return meanPrediction(xStar,initPos);
}
MMatrix MotionSyn::generate(std::size_t identity,vector<std::size_t> contents,std::size_t interval)
{
std::size_t state_size = contents.size() * 2 - 1;
std::size_t length = interval * state_size;
MMatrix motion(length,mInitY.sizeCol());
std::vector<MMatrix> xStar(3);
xStar[0].resize(length, 2);
xStar[1].resize(length, mFactors[1]->sizeCol());
xStar[2].resize(length, mFactors[2]->sizeCol());
double current_state = 0;
for (std::size_t i = 0; i < state_size; i++)
{
MMatrix kron(1,mFactors[1]->sizeCol() * mFactors[2]->sizeCol());
if (i % 2 == 0)
{
MMatrix mat1 = mFactors[1]->subMMatrix(identity, 0, 1, mFactors[1]->sizeCol());
MMatrix mat2 = mFactors[2]->subMMatrix(contents[i/2], 0, 1, mFactors[2]->sizeCol());
kron = mat1.kron(mat2);
MMatrix val = mGPM->predict(kron);
double step = val.get(0);
for (std::size_t t = 0; t < interval; t++)
{
xStar[0].assign(cos(current_state + double(t*step)), t + i * interval, 0);
xStar[0].assign(sin(current_state + double(t*step)), t + i * interval, 1);
xStar[1].copyRowRow(t + i * interval, *mFactors[1], identity);
xStar[2].copyRowRow(t + i * interval, *mFactors[2], contents[i/2]);
}
current_state += step * interval;
}
else
{
for (std::size_t t = 0; t < interval; t++)
{
MMatrix linearIpconent(1,mFactors[2]->sizeCol());
for (std::size_t k = 0; k < linearIpconent.sizeCol(); k++)
{
double val = (1 - double(t) / interval) * mFactors[2]->get(contents[(i-1)/2], k)
+ (double(t) / interval) * mFactors[2]->get(contents[(i+1)/2], k);
linearIpconent.assign(val, 0, k);
}
MMatrix mat = mFactors[1]->subMMatrix(identity, 0, 1, mFactors[1]->sizeCol());
kron = mat.kron(linearIpconent);
MMatrix val = mGPM->predict(kron);
double step = val.get(0);
current_state += step;
xStar[0].assign(cos(current_state), t + i * interval, 0);
xStar[0].assign(sin(current_state), t + i * interval, 1);
xStar[1].copyRowRow(t + i * interval, *mFactors[1], identity);
xStar[2].copyRowRow(t + i * interval, linearIpconent, 0);
}
}
}
return meanPrediction(xStar,CVector3D<double>(0,mInitY.get(identity,1),0));
}
