void SimilarityTrans::transform(const ShapeVec &src, ShapeVec &dst) const{
int nP = src.nPoints();
dst.create(nP<<1, 1);
double xt, yt;
for (int i=0; i < nP; i++) {
xt = src.X(i);
yt = src.Y(i);
dst.X(i) = a * xt - b * yt + Xt;
dst.Y(i) = b * xt + a * yt + Yt;
}
}
void SimilarityTrans::invTransform(const ShapeVec &src, ShapeVec &dst) const{
int nP = src.nPoints();
double x11, x12, x21, x22;
x11 = a/(a*a+b*b);
x12 = b/(a*a+b*b);
x21 = -b/(a*a+b*b);
x22 = a/(a*a+b*b);
dst.create(nP<<1, 1);
double xt, yt;
for (int i=0; i < nP; i++) {
xt = src.X(i) - Xt;
yt = src.Y(i) - Yt;
dst.X(i) = x11 * xt + x12 * yt;
dst.Y(i) = x21 * xt + x22 * yt;
}
}