double compute2DPose(const std::vector< std::pair<Point2D, Point2D> > &correspondences, OrientedPoint2D& transformation)
{
Point2D point1mean, point2mean;
for(unsigned int i = 0; i < correspondences.size(); i++){
point1mean = point1mean + correspondences[i].first;
point2mean = point2mean + correspondences[i].second;
}
point1mean = point1mean * (1./double(correspondences.size()));
point2mean = point2mean * (1./double(correspondences.size()));
double A = 0, B = 0;
for(unsigned int i = 0; i < correspondences.size(); i++){
Point2D delta1 = correspondences[i].first - point1mean;
Point2D delta2 = correspondences[i].second - point2mean;
A += delta1 * delta2;
B += delta1.ortho() * delta2;
}
A /= double(correspondences.size());
B /= double(correspondences.size());
double denom = sqrt(A*A + B*B);
double sinalpha1 = B/denom;
double cosalpha1 = -A/denom;
double sinalpha2 = -sinalpha1;
double cosalpha2 = -cosalpha1;
Point2D point1rotated(cosalpha1 * point1mean.x - sinalpha1 * point1mean.y,
sinalpha1 * point1mean.x + cosalpha1 * point1mean.y);
Point2D point2rotated(cosalpha2 * point1mean.x - sinalpha2 * point1mean.y,
sinalpha2 * point1mean.x + cosalpha2 * point1mean.y);
Point2D translation1(point2mean - point1rotated);
Point2D translation2(point2mean - point2rotated);
double error1 = 0, error2 = 0;
for(unsigned int i = 0; i < correspondences.size(); i++){
const Point2D& point1 = correspondences[i].first;
const Point2D& point2 = correspondences[i].second;
Point2D delta1 = point2 - Point2D(cosalpha1 * point1.x - sinalpha1 * point1.y, sinalpha1 * point1.x + cosalpha1 * point1.y) - translation1;
Point2D delta2 = point2 - Point2D(cosalpha2 * point1.x - sinalpha2 * point1.y, sinalpha2 * point1.x + cosalpha2 * point1.y) - translation2;
error1 = error1 + delta1 * delta1;
error2 = error2 + delta2 * delta2;
}
if(error1 <= error2){
transformation.x = translation1.x; transformation.y = translation1.y; transformation.theta = atan2(sinalpha1, cosalpha1);
return error1;
} else {
transformation.x = translation2.x; transformation.y = translation2.y; transformation.theta = atan2(sinalpha2, cosalpha2);
return error2;
}
}
void npBoneMesh::update(float timeStep,float _animeSlerp,int ani1,int ani2){
int anime1 =ani1;
int anime2 =ani2;
float animeSlerp =_animeSlerp;
//rootBone
time+=timeStep;
if (time> bones[1]->animations[1].totalTime)time =0;
for (int i=0;i< bones.size();i++)
{
bones[i]->animeMatrix.makeIdentityMatrix();
if (bones[i]->animations.size()>0)
{
ofQuaternion quat;
ofQuaternion quat1;
quat1.set(1,0,0,0);
if (bones[i]->animations[anime1].rotations.size() ==1)
{
quat1 = bones[i]->animations[anime1].rotations[0].quat;
}
else
{
for (int j=1; j<bones[i]->animations[anime1].rotations.size() ;j++)
{
if (bones[i]->animations[anime1].rotations[j].time >time)
{
float time1 = bones[i]->animations[anime1].rotations[j-1].time ;
float timeTotal = bones[i]->animations[anime1].rotations[j].time -time1 ;
float timeCurrent = time-time1;
float timeslerp = timeCurrent /timeTotal;
quat1.slerp(timeslerp , bones[i]->animations[anime1].rotations[j-1].quat, bones[i]->animations[anime1].rotations[j].quat);
break;
}
}
}
ofQuaternion quat2;
quat2.set(1,0,0,0);
if (bones[i]->animations[anime2].rotations.size() ==1)
{
quat2 = bones[i]->animations[anime2].rotations[0].quat;
}
else
{
for (int j=1; j<bones[i]->animations[anime2].rotations.size() ;j++)
{
if (bones[i]->animations[anime2].rotations[j].time >time)
{
float time1 = bones[i]->animations[anime2].rotations[j-1].time ;
float timeTotal = bones[i]->animations[anime2].rotations[j].time -time1 ;
float timeCurrent = time-time1;
float timeslerp = timeCurrent /timeTotal;
quat2.slerp(timeslerp , bones[i]->animations[anime2].rotations[j-1].quat, bones[i]->animations[anime2].rotations[j].quat);
break;
}
}
}
quat.slerp(animeSlerp , quat1, quat2);
if (i==4) quat.makeRotate(0,ofVec3f(0,1,0));
bones[i]->animeMatrix.makeRotationMatrix(quat);
ofVec3f translation(0,0,0);
ofVec3f translation1(0,0,0);
ofVec3f translation2(0,0,0);
if (bones[i]->animations[0].translations.size()>0)
{
if (bones[i]->animations[anime1].translations.size() ==1)
{
translation1 = bones[i]->animations[anime1].translations[0].pos;
}else
{
for (int j=1; j<bones[i]->animations[anime1].translations.size() ;j++)
{
if (bones[i]->animations[anime1].translations[j].time >time)
{
float time1 = bones[i]->animations[anime1].translations[j-1].time ;
float timeTotal = bones[i]->animations[anime1].translations[j].time -time1 ;
float timeCurrent = time-time1;
float timeslerp = timeCurrent /timeTotal;
translation1 = bones[i]->animations[anime1].translations[j-1].pos *(1.0f -timeslerp) +bones[i]->animations[anime1].translations[j].pos *(timeslerp);
break;
}
}
}
if (bones[i]->animations[anime2].translations.size() ==1)
{
translation2= bones[i]->animations[anime2].translations[0].pos;
}else
{
for (int j=1; j<bones[i]->animations[anime2].translations.size() ;j++)
{
if (bones[i]->animations[anime2].translations[j].time >time)
{
float time1 = bones[i]->animations[anime2].translations[j-1].time ;
float timeTotal = bones[i]->animations[anime2].translations[j].time -time1 ;
float timeCurrent = time-time1;
float timeslerp = timeCurrent /timeTotal;
translation2 = bones[i]->animations[anime2].translations[j-1].pos *(1.0f -timeslerp) +bones[i]->animations[anime2].translations[j].pos *(timeslerp);
break;
}
}
}
translation= translation1 *(1.0f -animeSlerp) +translation2 *(animeSlerp);
bones[i]->animeMatrix.postMultTranslate(translation);
}
}
}
for (int i=0;i< bones.size();i++)
{
bones[i]->finalMatrix=bones[i]->boneMatrix;
const npBone* tempNode = bones[i];
while( tempNode)
{
// check your matrix multiplication order here!!!
bones[i]->finalMatrix =bones[i]->finalMatrix * tempNode->animeMatrix;
tempNode = tempNode->parent;
}
//bones[i]->finalMatrix =bones[i]->animeMatrix;
};
setMatrixes();
}
