luke
/
klayout
mirror of https://github.com/KLayout/klayout.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
klayout/src/db/dbMatrix.cc

908 lines
25 KiB
C++
Raw Blame History

/*
KLayout Layout Viewer
Copyright (C) 2006-2017 Matthias Koefferlein
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "dbMatrix.h"
#include "dbTrans.h"
#include "tlString.h"
#include <sstream>
#include <limits>
#include <cmath>
namespace db
{
inline double mnorm (double x)
{
return fabs (x) < 1e-14 ? 0.0 : x;
}
std::string
Matrix2d::to_string () const
{
return tl::sprintf ("(%.12g,%.12g) (%.12g,%.12g)", mnorm (m_m11), mnorm (m_m12), mnorm (m_m21), mnorm (m_m22));
}
std::pair<double, double>
Matrix2d::mag () const
{
double s1 = sqrt (m_m11 * m_m11 + m_m21 * m_m21);
double s2 = sqrt (m_m12 * m_m12 + m_m22 * m_m22);
double n = sqrt (fabs (det ()) / (s1 * s2));
return std::make_pair (n * s1, n * s2);
}
bool
Matrix2d::has_rotation () const
{
return fabs (m_m11 - 1.0) > 1e-10 || fabs (m_m12) > 1e-10 || fabs (m_m21) > 1e-10 || fabs (m_m22 - 1.0) > 1e-10;
}
double
Matrix2d::angle () const
{
std::pair <double, double> m = mag ();
double u1 = m.first;
double u2 = is_mirror () ? -m.second : m.second;
double n11 = m_m11 / u1;
double n12 = m_m12 / u2;
double n21 = m_m21 / u1;
double n22 = m_m22 / u2;
// due to rounding error, the sqrt argument can be a negative (but very small)
double sin_a = 0.5 * sqrt (std::max (0.0, (n21 - n12) * (n21 - n12) - (n11 - n22) * (n11 - n22)));
double cos_a = 0.5 * sqrt (std::max (0.0, (n11 + n22) * (n11 + n22) - (n21 + n12) * (n21 + n12)));
if (n11 + n22 < 0.0) {
cos_a = -cos_a;
}
if (n21 - n12 < 0.0) {
sin_a = -sin_a;
}
return 180.0 * atan2 (sin_a, cos_a) / M_PI;
}
Matrix2d
Matrix2d::rotation (double a)
{
a *= M_PI / 180.0;
return Matrix2d (cos (a), -sin (a), sin (a), cos (a));
}
bool
Matrix2d::has_shear () const
{
std::pair <double, double> m = mag ();
double u1 = m.first;
double u2 = is_mirror () ? -m.second : m.second;
double n11 = m_m11 / u1;
double n12 = m_m12 / u2;
double n21 = m_m21 / u1;
double n22 = m_m22 / u2;
double fsin_a = 0.5 * sqrt ((n21 + n12) * (n21 + n12) + (n11 - n22) * (n11 - n22));
return fabs (fsin_a) > 1e-10;
}
double
Matrix2d::shear_angle () const
{
std::pair <double, double> m = mag ();
double u1 = m.first;
double u2 = is_mirror () ? -m.second : m.second;
double n11 = m_m11 / u1;
double n12 = m_m12 / u2;
double n21 = m_m21 / u1;
double n22 = m_m22 / u2;
double fsin_a = 0.5 * sqrt ((n21 + n12) * (n21 + n12) + (n11 - n22) * (n11 - n22));
double fcos_a = 0.5 * sqrt ((n21 - n12) * (n21 - n12) + (n11 + n22) * (n11 + n22));
if ((n21 - n12) * (n22 - n11) < -1e-10 || (n21 + n12) * (n11 + n22) < -1e-10) {
fsin_a = -fsin_a;
}
return 180.0 * atan2 (fsin_a, fcos_a) / M_PI;
}
Matrix2d
Matrix2d::shear (double a)
{
a *= M_PI / 180.0;
double cos_a = cos (a);
double sin_a = sin (a);
double f = 1.0 / sqrt (cos_a * cos_a - sin_a * sin_a);
return Matrix2d (f * cos_a, f * sin_a, f * sin_a, f * cos_a);
}
bool
Matrix2d::is_ortho () const
{
return fabs (m_m11 * m_m12 + m_m21 * m_m22) < 1e-10 && fabs (m_m11 * m_m12) < 1e-10 && fabs (m_m21 * m_m22) < 1e-10;
}
bool
Matrix2d::equal (const Matrix2d &d) const
{
return fabs (m_m11 - d.m_m11) < 1e-10 && fabs (m_m12 - d.m_m12) < 1e-10 &&
fabs (m_m21 - d.m_m21) < 1e-10 && fabs (m_m22 - d.m_m22) < 1e-10;
}
bool
Matrix2d::less (const Matrix2d &d) const
{
if (fabs (m_m11 - d.m_m11) > 1e-10) {
return m_m11 < d.m_m11;
}
if (fabs (m_m12 - d.m_m12) > 1e-10) {
return m_m12 < d.m_m12;
}
if (fabs (m_m21 - d.m_m21) > 1e-10) {
return m_m21 < d.m_m21;
}
if (fabs (m_m22 - d.m_m22) > 1e-10) {
return m_m22 < d.m_m22;
}
return false;
}
// --------------------------------------------------------------------------------------------
double
Matrix3d::det () const
{
double d = 0.0;
for (int i0 = 0; i0 < 3; ++i0) {
for (int j = 0; j < 2; ++j) {
int i1 = (i0 + j + 1) % 3;
int i2 = (i1 + 1) % 3;
double s = (((i0 + i1 + i2) % 2) == 0) ? -1.0 : 1.0;
d += s * m_m [0][i0] * m_m [1][i1] * m_m [2][i2];
}
}
return d;
}
db::DVector
Matrix3d::trans (const db::DPoint &p, const db::DVector &v) const
{
double t[2][2];
for (int i = 0; i < 2; ++i) {
for (int j = 0; j < 2; ++j) {
t[i][j] = (m_m[i][j] * m_m[2][1 - j] - m_m[i][1 - j] * m_m[2][j]) * (j == 0 ? p.y() : p.x()) + (m_m[2][2] * m_m[i][j] - m_m[i][2] * m_m[2][j]);
}
}
return db::DVector(v.x() * t[0][0] + v.y() * t[0][1], v.x() * t[1][0] + v.y() * t[1][1]);
}
bool
Matrix3d::can_transform (const db::DPoint &p) const
{
double r[3] = { 0, 0, 0 };
for (int i = 0; i < 3; ++i) {
r[i] = m_m[i][0] * p.x() + m_m[i][1] * p.y() + m_m[i][2];
}
return (r[2] > (std::abs (r[0]) + std::abs (r[1])) * 1e-10);
}
db::DPoint
Matrix3d::trans (const db::DPoint &p) const
{
double r[3] = { 0, 0, 0 };
for (int i = 0; i < 3; ++i) {
r[i] = m_m[i][0] * p.x() + m_m[i][1] * p.y() + m_m[i][2];
}
// safe approximation to the forbidden area where z <= 0
double z = std::max (r [2], (std::abs (r[0]) + std::abs (r[1])) * 1e-10);
return db::DPoint (r[0] / z, r[1] / z);
}
Matrix3d
Matrix3d::inverted () const
{
double m[3][3];
Matrix3d r (1.0);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
m[i][j] = m_m[i][j];
}
}
for (int i = 0; i < 2; ++i) {
for (int j = 2; j > i; --j) {
if (std::abs (m[j][i]) > std::abs (m[j - 1][i])) {
for (int k = 0; k < 3; ++k) {
std::swap (m[j][k], m[j - 1][k]);
std::swap (r.m_m[j][k], r.m_m[j - 1][k]);
}
}
}
}
for (int i = 0; i < 3; ++i) {
for (int j = i + 1; j < 3; ++j) {
double f = m[j][i] / m[i][i];
for (int k = 0; k < 3; ++k) {
m[j][k] -= f * m[i][k];
r.m_m[j][k] -= f * r.m_m[i][k];
}
}
double f = 1.0 / m[i][i];
for (int k = 0; k < 3; ++k) {
m[i][k] *= f;
r.m_m[i][k] *= f;
}
}
for (int i = 2; i > 0; --i) {
for (int j = 0; j < i; ++j) {
double f = m[j][i];
for (int k = 0; k < 3; ++k) {
r.m_m[j][k] -= f * r.m_m[i][k];
}
}
}
return r;
}
db::DVector
Matrix3d::disp () const
{
return db::DVector (m_m[0][2] / m_m[2][2], m_m[1][2] / m_m[2][2]);
}
double
Matrix3d::perspective_tilt_x (double z) const
{
db::DVector d = disp ();
db::Matrix3d m = db::Matrix3d::disp (-d) * *this;
return 180 * atan (z * (m.m ()[2][0] * m.m ()[1][1] - m.m ()[2][1] * m.m ()[1][0]) / (m.m ()[0][0] * m.m ()[1][1] - m.m ()[0][1] * m.m ()[1][0])) / M_PI;
}
double
Matrix3d::perspective_tilt_y (double z) const
{
db::DVector d = disp ();
db::Matrix3d m = db::Matrix3d::disp (-d) * *this;
return 180 * atan (z * (m.m ()[2][1] * m.m ()[0][0] - m.m ()[2][0] * m.m ()[0][1]) / (m.m ()[0][0] * m.m ()[1][1] - m.m ()[0][1] * m.m ()[1][0])) / M_PI;
}
bool
Matrix3d::has_perspective () const
{
return fabs (m_m[2][0]) + fabs (m_m[2][1]) > 1e-10;
}
Matrix3d
Matrix3d::perspective (double tx, double ty, double z)
{
tx *= M_PI / 180.0;
ty *= M_PI / 180.0;
return Matrix3d (1.0, 0.0, 0.0, 1.0, 0.0, 0.0, tan (tx) / z, tan (ty) / z);
}
Matrix2d
Matrix3d::m2d () const
{
db::DVector d = disp ();
db::Matrix3d m = db::Matrix3d::disp (-d) * *this;
if (has_perspective ()) {
m = Matrix3d::perspective (-perspective_tilt_x (1.0), -perspective_tilt_y (1.0), 1.0) * m;
}
return Matrix2d (m.m_m[0][0] / m.m_m[2][2], m.m_m[0][1] / m.m_m[2][2], m.m_m[1][0] / m.m_m[2][2], m.m_m[1][1] / m.m_m[2][2]);
}
std::string
Matrix3d::to_string () const
{
return tl::sprintf ("(%.12g,%.12g,%.12g)", mnorm (m_m[0][0]), mnorm (m_m[0][1]), mnorm (m_m[0][2])) + " "
+ tl::sprintf ("(%.12g,%.12g,%.12g)", mnorm (m_m[1][0]), mnorm (m_m[1][1]), mnorm (m_m[1][2])) + " "
+ tl::sprintf ("(%.12g,%.12g,%.12g)", mnorm (m_m[2][0]), mnorm (m_m[2][1]), mnorm (m_m[2][2]));
}
bool
Matrix3d::is_ortho () const
{
return ! has_perspective () && m2d ().is_ortho ();
}
bool
Matrix3d::equal (const Matrix3d &d) const
{
for (unsigned int i = 0; i < 3; ++i) {
for (unsigned int j = 0; j < 3; ++j) {
if (fabs (m_m[i][j] - d.m_m[i][j]) > 1e-10) {
return false;
}
}
}
return true;
}
bool
Matrix3d::less (const Matrix3d &d) const
{
for (unsigned int i = 0; i < 3; ++i) {
for (unsigned int j = 0; j < 3; ++j) {
if (fabs (m_m[i][j] - d.m_m[i][j]) > 1e-10) {
return m_m[i][j] < d.m_m[i][j];
}
}
}
return false;
}
// --------------------------------------------------------------------------------------------
/**
* @brief Fits a vector set to another using a linear transformation that is a linear combination of two: M = a*A + b*B
*
* It returns the best-fit parameters a and b.
*
* @param am The matrix A
* @param bm The matrix B
* @param p The initial vector set
* @param q The vector set to fit M*p[i] to
*/
static bool
fit_point_set_with_linear_combination (double &a, double &b, const Matrix2d &am, const Matrix2d &bm, const std::vector<db::DVector> &q, const std::vector<db::DVector> &p)
{
double maa = 0.0, mbaab = 0.0, mbb = 0.0, ca = 0.0, cb = 0.0;
for (size_t i = 0; i < p.size (); ++i) {
db::DVector ap = am * p[i];
db::DVector bp = bm * p[i];
maa += ap.sq_double_length ();
mbaab += db::sprod (bp, ap);
mbb += bp.sq_double_length ();
ca += db::sprod (q [i], ap);
cb += db::sprod (q [i], bp);
}
Matrix2d m (maa, mbaab, mbaab, mbb);
if (fabs (m.det ()) < 1e-10) {
// fit not possible
return false;
} else {
db::DVector r = Matrix2d (maa, mbaab, mbaab, mbb).inverted () * db::DVector (ca, cb);
a = r.x ();
b = r.y ();
return true;
}
}
/**
* @brief Computes the cost value for a given matrix (sum over the square distances)
*
* @param m The matrix
* @param p The initial vector set
* @param q The vector set to fit M*p[i] to
*/
static double
compute_distance (const Matrix2d &m, const std::vector<db::DVector> &q, const std::vector<db::DVector> &p)
{
double d = 0.0;
for (size_t i = 0; i < p.size (); ++i) {
d += (q[i] - m * p[i]).sq_double_length ();
}
return d;
}
void
adjust_matrix (Matrix2d &matrix, db::DVector &disp, const std::vector <db::DPoint> &landmarks_before, const std::vector <db::DPoint> &landmarks_after, MatrixAdjustFlags::Flags flags, int fixed_point)
{
tl_assert (landmarks_before.size () == landmarks_after.size ());
if (flags > MatrixAdjustFlags::Shear) {
flags = MatrixAdjustFlags::Shear;
}
if (landmarks_before.size () == 0) {
flags = MatrixAdjustFlags::None;
} else if (landmarks_before.size () == 1) {
flags = std::min (MatrixAdjustFlags::Displacement, flags);
} else if (landmarks_before.size () == 2) {
flags = std::min (MatrixAdjustFlags::Magnification, flags);
} else if (landmarks_before.size () == 3) {
flags = std::min (MatrixAdjustFlags::Shear, flags);
}
// Don't do anything in "none" mode.
if (flags == MatrixAdjustFlags::None) {
return;
}
// Determine initial center of weight or fixed displacement (if there is a fixed point) before and after.
// Extract the displacement so we have a 0,0-centered vector set.
// Use the initial transformation as a basis for the adjustment.
db::DPoint dp;
if (fixed_point >= 0 && fixed_point < int (landmarks_before.size ())) {
dp = matrix * landmarks_before [fixed_point] + disp;
} else {
for (std::vector<db::DPoint>::const_iterator p = landmarks_before.begin (); p != landmarks_before.end (); ++p) {
dp += matrix * db::DVector (*p) + disp;
}
dp *= 1.0 / double (landmarks_before.size ());
}
std::vector<db::DVector> p;
p.reserve (landmarks_before.size ());
for (std::vector<db::DPoint>::const_iterator pp = landmarks_before.begin (); pp != landmarks_before.end (); ++pp) {
p.push_back ((matrix * *pp + disp) - dp);
}
db::DPoint dq;
if (fixed_point >= 0 && fixed_point < int (landmarks_after.size ())) {
dq = landmarks_after [fixed_point];
} else {
for (std::vector<db::DPoint>::const_iterator q = landmarks_after.begin (); q != landmarks_after.end (); ++q) {
dq += *q - db::DPoint ();
}
dq *= 1.0 / double (landmarks_after.size ());
}
std::vector<db::DVector> q;
q.reserve (landmarks_after.size ());
for (std::vector<db::DPoint>::const_iterator qq = landmarks_after.begin (); qq != landmarks_after.end (); ++qq) {
q.push_back (*qq - dq);
}
// In the special case of RotationMirror and exactly 3 points with a fixed point adjust the
// other so they form normal vectors to the fixed point. In that case, the intention is very likely to
// define two axes with their length being not important.
if (flags == MatrixAdjustFlags::RotationMirror && landmarks_after.size () == 3 && fixed_point >= 0 && fixed_point < 3) {
for (int i = 0; i < 3; ++i) {
if (i != fixed_point) {
double n;
n = p [i].double_length ();
if (fabs (n) > 1e-6) {
p [i] *= 1.0 / n;
}
n = q [i].double_length ();
if (fabs (n) > 1e-6) {
q [i] *= 1.0 / n;
}
}
}
}
// Extract a Matrix2d rotation matrix according to the extraction mode
Matrix2d m (1.0, 0.0, 0.0, 1.0);
if (flags == MatrixAdjustFlags::Shear) {
// Do a full fit
Matrix2d pm;
Matrix2d qm;
for (size_t i = 0; i < p.size (); ++i) {
double px = p [i].x ();
double py = p [i].y ();
double qx = q [i].x ();
double qy = q [i].y ();
pm += Matrix2d (px * px, px * py, py * px, py * py);
qm += Matrix2d (qx * px, qx * py, qy * px, qy * py);
}
// Fallback is Magnification mode if the fit cannot be performed
if (fabs (pm.det ()) < 1e-10) {
flags = MatrixAdjustFlags::Magnification;
} else {
m = qm * pm.inverted ();
}
}
if (flags == MatrixAdjustFlags::Displacement) {
// we are done now.
} else if (flags == MatrixAdjustFlags::Rotation || flags == MatrixAdjustFlags::Magnification) {
// Fit a general rotation matrix to the vector sets and drop the magnification part
Matrix2d am (1.0, 0.0, 0.0, 1.0), bm (0.0, -1.0, 1.0, 0.0);
double a = 0.0, b = 0.0;
if (fit_point_set_with_linear_combination (a, b, am, bm, q, p) && fabs (a) + fabs (b) > 1e-6) {
double n = flags == MatrixAdjustFlags::Magnification ? 1.0 : 1.0 / sqrt (a * a + b * b);
m = am * (a * n) + bm * (b * n);
}
} else if (flags == MatrixAdjustFlags::RotationMirror || flags == MatrixAdjustFlags::MagnificationMirror) {
// Same as before but perform two tries (with and without mirror) and don't drop the magnification part
// if magnification adjustment is requested.
Matrix2d am1 (1.0, 0.0, 0.0, 1.0), bm1 (0.0, -1.0, 1.0, 0.0);
double a1 = 0.0, b1 = 0.0;
double d1 = std::numeric_limits<double>::max ();
if (fit_point_set_with_linear_combination (a1, b1, am1, bm1, q, p) && fabs (a1) + fabs (b1) > 1e-6) {
d1 = compute_distance (am1 * a1 + bm1 * b1, q, p);
}
Matrix2d am2 (1.0, 0.0, 0.0, -1.0), bm2 (0.0, 1.0, 1.0, 0.0);
double a2 = 0.0, b2 = 0.0;
double d2 = std::numeric_limits<double>::max ();
if (fit_point_set_with_linear_combination (a2, b2, am2, bm2, q, p) && fabs (a2) + fabs (b2) > 1e-6) {
d2 = compute_distance (am2 * a2 + bm2 * b2, q, p);
}
if (d1 < std::numeric_limits<double>::max () || d2 < std::numeric_limits<double>::max ()) {
if (d1 < d2 + 1e-10) {
double n = flags == MatrixAdjustFlags::MagnificationMirror ? 1.0 : 1.0 / sqrt (a1 * a1 + b1 * b1);
m = am1 * (a1 * n) + bm1 * (b1 * n);
} else {
double n = flags == MatrixAdjustFlags::MagnificationMirror ? 1.0 : 1.0 / sqrt (a2 * a2 + b2 * b2);
m = am2 * (a2 * n) + bm2 * (b2 * n);
}
}
}
// compute the final transformation
disp = (dq - db::DPoint ()) + m * (disp - (dp - db::DPoint ()));
matrix = m * matrix;
}
void
adjust_matrix (Matrix3d &matrix, const std::vector <db::DPoint> &landmarks_before, const std::vector <db::DPoint> &landmarks_after, MatrixAdjustFlags::Flags flags, int fixed_point)
{
tl_assert (landmarks_before.size () == landmarks_after.size ());
if (flags > MatrixAdjustFlags::Perspective) {
flags = MatrixAdjustFlags::Perspective;
}
if (landmarks_before.size () == 0) {
flags = MatrixAdjustFlags::None;
} else if (landmarks_before.size () == 1) {
flags = std::min (MatrixAdjustFlags::Displacement, flags);
} else if (landmarks_before.size () == 2) {
flags = std::min (MatrixAdjustFlags::Magnification, flags);
} else if (landmarks_before.size () == 3) {
flags = std::min (MatrixAdjustFlags::Shear, flags);
}
if (flags < MatrixAdjustFlags::Perspective) {
if (matrix.has_perspective ()) {
std::vector<db::DPoint> p;
p.reserve (landmarks_before.size ());
for (std::vector<db::DPoint>::const_iterator pp = landmarks_before.begin (); pp != landmarks_before.end (); ++pp) {
p.push_back (matrix * *pp);
}
Matrix2d m2d (1.0);
db::DVector d2d;
adjust_matrix (m2d, d2d, p, landmarks_after, flags, fixed_point);
matrix = Matrix3d::disp (d2d) * Matrix3d (m2d) * matrix;
} else {
Matrix2d m2d (matrix.m2d ());
db::DVector d2d (matrix.disp ());
adjust_matrix (m2d, d2d, landmarks_before, landmarks_after, flags, fixed_point);
matrix = Matrix3d::disp (d2d) * Matrix3d (m2d);
}
} else {
// Determine initial center of weight or fixed displacement (if there is a fixed point) before and after.
// Extract the displacement so we have a 0,0-centered vector set.
// Use the initial transformation as a basis for the adjustment.
db::DPoint dp;
if (fixed_point >= 0 && fixed_point < int (landmarks_before.size ())) {
dp = matrix * landmarks_before [fixed_point];
}
std::vector<db::DVector> p;
p.reserve (landmarks_before.size ());
for (std::vector<db::DPoint>::const_iterator pp = landmarks_before.begin (); pp != landmarks_before.end (); ++pp) {
p.push_back (matrix * *pp - dp);
}
db::DPoint dq;
if (fixed_point >= 0 && fixed_point < int (landmarks_after.size ())) {
dq = landmarks_after [fixed_point];
}
std::vector<db::DVector> q;
q.reserve (landmarks_after.size ());
for (std::vector<db::DPoint>::const_iterator qq = landmarks_after.begin (); qq != landmarks_after.end (); ++qq) {
q.push_back (*qq - dq);
}
// Perform the fit of m11, m12, m13, m21, m22, m23, m31, m32.
// We needs to solve a set of 8 linear equations whose coefficients we collect in l and whose right side we collect in r.
double l[8][8];
double r[8];
for (int i = 0; i < 8; ++i) {
r[i] = 0.0;
for (int j = 0; j < 8; ++j) {
l[i][j] = 0.0;
}
}
for (size_t n = 0; n < landmarks_after.size (); ++n) {
l[0][0] += p[n].x() * p[n].x();
l[0][1] += p[n].x() * p[n].y();
l[0][2] += p[n].x();
l[0][6] -= q[n].x() * p[n].x() * p[n].x();
l[0][7] -= q[n].x() * p[n].x() * p[n].y();
r[0] += q[n].x() * p[n].x();
l[1][0] += p[n].y() * p[n].x();
l[1][1] += p[n].y() * p[n].y();
l[1][2] += p[n].y();
l[1][6] -= q[n].x() * p[n].y() * p[n].x();
l[1][7] -= q[n].x() * p[n].y() * p[n].y();
r[1] += q[n].x() * p[n].y();
l[2][0] += p[n].x();
l[2][1] += p[n].y();
l[2][2] += 1.0;
l[2][6] -= q[n].x() * p[n].x();
l[2][7] -= q[n].x() * p[n].y();
r[2] += q[n].x();
l[3][3] += p[n].x() * p[n].x();
l[3][4] += p[n].x() * p[n].y();
l[3][5] += p[n].x();
l[3][6] -= q[n].y() * p[n].x() * p[n].x();
l[3][7] -= q[n].y() * p[n].x() * p[n].y();
r[3] += q[n].y() * p[n].x();
l[4][3] += p[n].y() * p[n].x();
l[4][4] += p[n].y() * p[n].y();
l[4][5] += p[n].y();
l[4][6] -= q[n].y() * p[n].y() * p[n].x();
l[4][7] -= q[n].y() * p[n].y() * p[n].y();
r[4] += q[n].y() * p[n].y();
l[5][3] += p[n].x();
l[5][4] += p[n].y();
l[5][5] += 1.0;
l[5][6] -= q[n].y() * p[n].x();
l[5][7] -= q[n].y() * p[n].y();
r[5] += q[n].y();
l[6][0] += q[n].x() * p[n].x() * p[n].x();
l[6][1] += q[n].x() * p[n].x() * p[n].y();
l[6][2] += q[n].x() * p[n].x();
l[6][3] += q[n].y() * p[n].x() * p[n].x();
l[6][4] += q[n].y() * p[n].x() * p[n].y();
l[6][5] += q[n].y() * p[n].x();
l[6][6] -= q[n].sq_double_length () * p[n].x() * p[n].x();
l[6][7] -= q[n].sq_double_length () * p[n].x() * p[n].y();
r[6] += q[n].sq_double_length () * p[n].x();
l[7][0] += q[n].x() * p[n].y() * p[n].x();
l[7][1] += q[n].x() * p[n].y() * p[n].y();
l[7][2] += q[n].x() * p[n].y();
l[7][3] += q[n].y() * p[n].y() * p[n].x();
l[7][4] += q[n].y() * p[n].y() * p[n].y();
l[7][5] += q[n].y() * p[n].y();
l[7][6] -= q[n].sq_double_length () * p[n].y() * p[n].x();
l[7][7] -= q[n].sq_double_length () * p[n].y() * p[n].y();
r[7] += q[n].sq_double_length () * p[n].y();
}
// Now solve the equation system
for (int i = 0; i < 7; ++i) {
for (int j = 7; j > i; --j) {
if (std::abs (l[j][i]) > std::abs (l[j - 1][i])) {
for (int k = 0; k < 8; ++k) {
std::swap (l[j][k], l[j - 1][k]);
}
std::swap (r[j], r[j - 1]);
}
}
}
for (int i = 0; i < 8; ++i) {
for (int j = i + 1; j < 8; ++j) {
double f = l[j][i] / l[i][i];
for (int k = 0; k < 8; ++k) {
l[j][k] -= f * l[i][k];
}
r[j] -= f * r[i];
}
double f = 1.0 / l[i][i];
for (int k = 0; k < 8; ++k) {
l[i][k] *= f;
}
r[i] *= f;
}
for (int i = 7; i > 0; --i) {
for (int j = 0; j < i; ++j) {
r[j] -= l[j][i] * r[i];
}
}
// compute the final transformation
matrix = Matrix3d::disp (dq - db::DPoint ()) * Matrix3d (r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7], 1.0) * Matrix3d::disp (db::DPoint () - dp) * matrix;
}
}
}
namespace tl
{
template<> bool test_extractor_impl (tl::Extractor &ex, db::Matrix2d &m)
{
double m11 = 0.0, m12 = 0.0, m21 = 0.0, m22 = 0.0;
if (! ex.test ("(")) {
return false;
}
if (! ex.try_read (m11)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m12)) {
return false;
}
if (! ex.test (")")) {
return false;
}
if (! ex.test ("(")) {
return false;
}
if (! ex.try_read (m21)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m22)) {
return false;
}
if (! ex.test (")")) {
return false;
}
m = db::Matrix2d (m11, m12, m21, m22);
return true;
}
template<> void extractor_impl (tl::Extractor &ex, db::Matrix2d &m)
{
if (! test_extractor_impl (ex, m)) {
ex.error (tl::to_string (QObject::tr ("Expected a 2d matrix specification")));
}
}
template<> bool test_extractor_impl (tl::Extractor &ex, db::Matrix3d &m)
{
double m11 = 0.0, m12 = 0.0, m13 = 0.0, m21 = 0.0, m22 = 0.0, m23 = 0.0, m31 = 0.0, m32 = 0.0, m33 = 0.0;
if (! ex.test ("(")) {
return false;
}
if (! ex.try_read (m11)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m12)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m13)) {
return false;
}
if (! ex.test (")")) {
return false;
}
if (! ex.test ("(")) {
return false;
}
if (! ex.try_read (m21)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m22)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m23)) {
return false;
}
if (! ex.test (")")) {
return false;
}
if (! ex.test ("(")) {
return false;
}
if (! ex.try_read (m31)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m32)) {
return false;
}
if (! ex.test (",")) {
return false;
}
if (! ex.try_read (m33)) {
return false;
}
if (! ex.test (")")) {
return false;
}
m = db::Matrix3d (m11, m12, m13, m21, m22, m23, m31, m32, m33);
return true;
}
template<> void extractor_impl (tl::Extractor &ex, db::Matrix3d &m)
{
if (! test_extractor_impl (ex, m)) {
ex.error (tl::to_string (QObject::tr ("Expected a 3d matrix specification")));
}
}
}
Reference in New Issue View Git Blame Copy Permalink