int SolveLinearSystemLS2 (const Vec3d & col1,
const Vec3d & col2,
const Vec2d & rhs,
Vec3d & sol, double & x, double & y)
{
double a11 = col1 * col1;
double a12 = col1 * col2;
double a22 = col2 * col2;
double det = a11 * a22 - a12 * a12;
if (fabs (det) <= 1e-12 * col1.Length() * col2.Length() ||
col1.Length2() == 0 || col2.Length2() == 0)
{
sol = Vec3d (0, 0, 0);
x = 0; y = 0;
return 1;
}
Vec2d invrhs;
invrhs.X() = ( a22 * rhs.X() - a12 * rhs.Y()) / det;
invrhs.Y() = (-a12 * rhs.X() + a11 * rhs.Y()) / det;
sol.X() = invrhs.X() * col1.X() + invrhs.Y() * col2.X();
sol.Y() = invrhs.X() * col1.Y() + invrhs.Y() * col2.Y();
sol.Z() = invrhs.X() * col1.Z() + invrhs.Y() * col2.Z();
x = invrhs.X();
y = invrhs.Y();
return 0;
/*
Vec3d inv1, inv2;
int err =
PseudoInverse (col1, col2, inv1, inv2);
sol = rhs.X() * inv1 + rhs.Y() * inv2;
return err;
*/
}
// 0 checks !!!
double MinDistLL2 (const Point3d & l1p1, const Point3d & l1p2,
const Point3d & l2p1, const Point3d & l2p2)
{
// dist(lam1,lam2) = \| l2p1+lam2v2 - (l1p1+lam1 v1) \|
// min !
Vec3d l1l2 (l1p1, l2p1);
Vec3d v1 (l1p1, l1p2);
Vec3d v2 (l2p1, l2p2);
double a11, a12, a22, rs1, rs2;
double lam1, lam2, det;
a11 = v1*v1;
a12 = -(v1*v2);
a22 = v2*v2;
rs1 = l1l2 * v1;
rs2 = - (l1l2 * v2);
det = a11 * a22 - a12 * a12;
if (det < 1e-14 * a11 * a22)
det = 1e-14 * a11 * a22; // regularization should be stable
if (det < 1e-20)
det = 1e-20;
lam1 = (a22 * rs1 - a12 * rs2) / det;
lam2 = (-a12 * rs1 + a11 * rs2) / det;
if (lam1 >= 0 && lam2 >= 0 && lam1 <= 1 && lam2 <= 1)
{
Vec3d v = l1l2 + (-lam1) * v1 + lam2 * v2;
return v.Length2();
}
double minv, hv;
minv = MinDistLP2 (l1p1, l1p2, l2p1);
hv = MinDistLP2 (l1p1, l1p2, l2p2);
if (hv < minv) minv = hv;
hv = MinDistLP2 (l2p1, l2p2, l1p1);
if (hv < minv) minv = hv;
hv = MinDistLP2 (l2p1, l2p2, l1p2);
if (hv < minv) minv = hv;
return minv;
}
