//define parameter computation
virtual void compute_param(){
Matrix2d M;
Vector2d b, *point, param;
list<Vector2d*>::iterator obs;
M << 0.0, 0.0, 0.0, 0.0;
b << 0.0, 0.0;
//check if we have enough data in fit_set
int ndata = (int)fit_set.size();
if (ndata >= nDataMin) {
//do linear least-squares fit
for (obs = fit_set.begin(); obs != fit_set.end(); obs++) {
point = *obs;
M(0,0) += 1;
M(0,1) += (*point)(0);
M(1,0) += (*point)(0);
M(1,1) += (*point)(0)*(*point)(0);
b(0) += (*point)(1);
b(1) += (*point)(0)*(*point)(1);
}
//calculate parameters
param = M.inverse()*b;
intercept = param(0);
slope = param(1);
}
};
//Check for passes through the left and right wall
vector<stick> make_ghost_lr( const vector<stick> &m, double LStick, int NumberMikado)
{
std::vector<stick> GhostLR;
for (int i=0;i<NumberMikado;i++){
if(m[i].th!=pi/2 || m[i].th!=3*pi/2){ // Check here for the left wall
stick ghostRowLR;
Matrix2d A; //The matrix to solve with
Vector2d b,b2; // A*st=b
Vector2d st,st2;
A<<-cos(m[i].th),0,-sin(m[i].th),1;
b<<m[i].x,m[i].y;
b2<<m[i].x-1,m[i].y;
st=A.lu().solve(b);
st2=A.lu().solve(b2);
if((st(0)>0 && st(0)<LStick) && (st(1)>0&&st(1)<1)){ //If mikado passes throug wall make ghost mikado
ghostRowLR=m[i];
ghostRowLR.x=ghostRowLR.x+1;
ghostRowLR.wlr=ghostRowLR.wlr-1;
GhostLR.push_back(ghostRowLR);
}
if((st2(0)>0&&st2(0)<LStick)&&(st2(1)>0&&st2(1)<1)){ //Now do the same for the upper wall
ghostRowLR=m[i];
ghostRowLR.x=ghostRowLR.x-1;
ghostRowLR.wlr=ghostRowLR.wlr+1;
GhostLR.push_back(ghostRowLR);
}
}
}
return GhostLR;
}
Vector2d leastSquareSolverCalib::pointAlignerLoop( Vector2d &x, MatrixXd &Z, int iterations, Vector2d &result)
{
result=x;
MatrixXd H(2,2);
H.setZero();
MatrixXd b(2,1);
b.setZero();
Vector2d X;
std::cout << Z.transpose()<< std::endl;
for(int i = 0; i < iterations; i++){
std::cout<<"iteration "<<i <<std::endl;
X=x;
for(int j=0;j<Z.cols();j++){
Matrix2d J = computeJacobian(j,Z);
Vector2d e=computeError(j,Z);
std:: cout << "error: "<< e<< std::endl<<std::endl;
H+=J.transpose()*J;
b+=J.transpose()*e;
}
}
Vector2d dx;
std::cout << "H: "<<std::endl<<H<<std::endl<<std::endl<<std::endl;
std::cout << "b: "<<std::endl<<b<<std::endl;
LDLT<MatrixXd> ldlt(-H);
dx=ldlt.solve(b); // using a LDLT factorizationldlt;
return dx;
}
//Check for passes through the down and up wall;
vector<stick> make_ghost_ud(const vector<stick> &m, double LStick, int NumberMikado){
vector<stick> GhostUD;
for(int i=0;i<NumberMikado;i++){
if(m[i].th!=0||m[i].th!=pi){
stick ghostRowUD;
Matrix2d A;
Vector2d b,b2;
Vector2d st, st2;
A<<-cos(m[i].th),1,-sin(m[i].th),0;
b<<m[i].x,m[i].y;
b2<<m[i].x,m[i].y-1;
st=A.lu().solve(b);
st2=A.lu().solve(b2);
if((st(0)>0&&st(0)<LStick)&&(st(1)>0&&st(1)<1)){
ghostRowUD=m[i];
ghostRowUD.y=ghostRowUD.y+1;
ghostRowUD.wud=ghostRowUD.wud-1;
GhostUD.push_back(ghostRowUD);
}
if((st2(0)>0&&st2(0)<LStick)&&(st2(1)>0&&st2(1)<1)){
ghostRowUD=m[i];
ghostRowUD.y=ghostRowUD.y-1;
ghostRowUD.wud=ghostRowUD.wud+1;
GhostUD.push_back(ghostRowUD);
}
}
}
return GhostUD;
}
MatrixXd Utils::calculateHomographyMatrixFromFiveOrtoghonalLines(QList<Line*> firstOrtoghonalLines, QList<Line*> secondOrthogonalLines,
QList<Line*> thirdOrthogonalLines, QList<Line*> fourthOrthogonalLines,
QList<Line*> fifthOrthogonalLines)
{
// A * x = b.
MatrixXd A(5, 6);
MatrixXd b(5, 1);
MatrixXd x(5, 1);
Vector3d l1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(0));
Vector3d m1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(1));
Vector3d l2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(0));
Vector3d m2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(1));
Vector3d l3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(0));
Vector3d m3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(1));
Vector3d l4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(0));
Vector3d m4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(1));
Vector3d l5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(0));
Vector3d m5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(1));
b << -l1(1)*m1(1), -l2(1)*m2(1), -l3(1)*m3(1), -l4(1)*m4(1), -l5(1)*m5(1);
A << l1(0)*m1(0), (l1(0)*m1(1)+l1(1)*m1(0))/2, l1(1)*m1(1), (l1(0)*m1(2)+l1(2)*m1(0))/2, (l1(1)*m1(2)+l1(2)*m1(1))/2, l1(2)*m1(2),
l2(0)*m2(0), (l2(0)*m2(1)+l2(1)*m2(0))/2, l2(1)*m2(1), (l2(0)*m2(2)+l2(2)*m2(0))/2, (l2(1)*m2(2)+l2(2)*m2(1))/2, l2(2)*m2(2),
l3(0)*m3(0), (l3(0)*m3(1)+l3(1)*m3(0))/2, l3(1)*m3(1), (l3(0)*m3(2)+l3(2)*m3(0))/2, (l3(1)*m3(2)+l3(2)*m3(1))/2, l3(2)*m3(2),
l4(0)*m4(0), (l4(0)*m4(1)+l4(1)*m4(0))/2, l4(1)*m4(1), (l4(0)*m4(2)+l4(2)*m4(0))/2, (l4(1)*m4(2)+l4(2)*m4(1))/2, l4(2)*m4(2),
l5(0)*m5(0), (l5(0)*m5(1)+l5(1)*m5(0))/2, l5(1)*m5(1), (l5(0)*m5(2)+l5(2)*m5(0))/2, (l5(1)*m5(2)+l5(2)*m5(1))/2, l5(2)*m5(2);
x = A.colPivHouseholderQr().solve(b);
x/=x(2);
Matrix3d C;
C << x(0), x(1)/2, x(3)/2,
x(1)/2, x(2), x(4)/2,
x(3)/2, x(4)/2, 1;
Matrix2d kkt;
kkt << C(0,0), C(0,1),
C(1,0), C(1,1);
MatrixXd vKKt(1,2);
vKKt << C(2,0), C(2,1);
MatrixXd V(1,2);
V = vKKt * kkt.inverse();
LLT<MatrixXd> llt(kkt);
MatrixXd U = llt.matrixU();
MatrixXd J (3,3);
J << U(0,0), U(0,1),0, U(1,0), U(1,1),0, V(0), V(1), 1;
return J;
}
template <typename T> std::ostream& operator << (std::ostream &OutStream, const Matrix2d<T> &MatrixObj)
{
Matrix2d<T> *pMatrixTmp =const_cast<Matrix2d<T> *>( &MatrixObj );
for(int i = 0; i < pMatrixTmp->getMatrix2dRow(); i++){
OutStream << "|"<< " ";
for(int j = 0; j < pMatrixTmp->getMatrix2dColumn(); j++){
OutStream << *(pMatrixTmp->getMatrix2dData() + i*pMatrixTmp->getMatrix2dColumn() + j)<< " ";
}
OutStream << "|" << std::endl;
}
return OutStream;
}
bool depthFromTriangulation(
const SE3& T_search_ref,
const Vector3d& f_ref,
const Vector3d& f_cur,
double& depth)
{
Matrix<double,3,2> A; A << T_search_ref.rotationMatrix() * f_ref, f_cur;
const Matrix2d AtA = A.transpose()*A;
if(AtA.determinant() < 0.000001)
return false;
const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
depth = fabs(depth2[0]);
return true;
}
/**
* This method is used to transpose the matrix
* @author Razmik Avetisyan
* @return Result of transpose operation
*/
Matrix2d Matrix2d::transpose()
{
Matrix2d data = m;
GLdouble *tmp = data.get();
int swap;
for(int i = 0; i < 3; i++)
for(int j = i+1; j < 3;j++){
swap = tmp[i*3+j];
tmp[i*3+j] = tmp[j*3+i];
tmp[j*3+i] = swap;
}
return data;
}
double Triangle<ConcreteShape>::estimatedElementRadius() {
Matrix2d invJ;
double detJ;
std::tie(invJ, detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
Matrix2d J = invJ.inverse();
VectorXcd eivals = J.eigenvalues();
// get minimum h (smallest direction)
Vector2d eivals_norm;
for(int i=0;i<2;i++) {
eivals_norm(i) = std::norm(eivals[i]);
}
return eivals_norm.minCoeff();
}
void warpAffine(
const Matrix2d& A_cur_ref,
const cv::Mat& img_ref,
const Vector2d& px_ref,
const int level_ref,
const int search_level,
const int halfpatch_size,
uint8_t* patch)
{
const int patch_size = halfpatch_size*2 ;
const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
if(isnan(A_ref_cur(0,0)))
{
printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
return;
}
// Perform the warp on a larger patch.
uint8_t* patch_ptr = patch;
const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
for (int y=0; y<patch_size; ++y)
{
for (int x=0; x<patch_size; ++x, ++patch_ptr)
{
Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
px_patch *= (1<<search_level);
const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
*patch_ptr = 0;
else
*patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
}
}
}
MatrixXd Triangle<ConcreteShape>::buildStiffnessMatrix(VectorXd velocity) {
Matrix2d invJ;
double detJ;
std::tie(invJ,detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
//Jinv= rx, sx,
// rz, sz;
auto drdx = invJ(0,0);
auto dsdx = invJ(0,1);
auto drdz = invJ(1,0);
auto dsdz = invJ(1,1);
// save for later reuse
mInvJac = invJ;
mInvJacT_x_invJac = invJ.transpose() * invJ;
mInvJacT = invJ.transpose();
mDetJac = detJ;
// build material on all nodes
MatrixXd elementStiffnessMatrix(mNumIntPnt,mNumIntPnt);
// interpolate velocity at all nodes
// for(int i=0;i<mNumIntPnt;i++) {
// auto r = mIntegrationCoordinates_r[i];
// auto s = mIntegrationCoordinates_s[i];
// velocity(i) = interpolateAtPoint(r,s).dot(mMaterialVelocityAtVertices);
// }
// loop over matrix(i,j)
for(int i=0;i<mNumIntPnt;i++) {
VectorXd dPhi_dr_i = mGradientPhi_dr.row(i);
VectorXd dPhi_ds_i = mGradientPhi_ds.row(i);
auto dPhi_dx_i = dPhi_dr_i*drdx + dPhi_ds_i*dsdx;
auto dPhi_dz_i = dPhi_dr_i*drdz + dPhi_ds_i*dsdz;
for(int j=0;j<mNumIntPnt;j++) {
VectorXd dPhi_dr_j = mGradientPhi_dr.row(j);
VectorXd dPhi_ds_j = mGradientPhi_ds.row(j);
auto dPhi_dx_j = dPhi_dr_j*drdx + dPhi_ds_j*dsdx;
auto dPhi_dz_j = dPhi_dr_j*drdz + dPhi_ds_j*dsdz;
elementStiffnessMatrix(i,j) = detJ*mIntegrationWeights.dot((velocity.array().pow(2) * dPhi_dx_i.array() * dPhi_dx_j.array()).matrix()) +
detJ*mIntegrationWeights.dot((velocity.array().pow(2) * dPhi_dz_i.array() * dPhi_dz_j.array()).matrix());
}
}
return elementStiffnessMatrix;
}
void updateTransforms()
{
w2dx = viewScale * dpiX / 25.4f;
w2dy = viewScale * dpiY / 25.4f;
float wdy = ydown ? -w2dy : w2dy;
float xc = cxWnd * 0.5f;
float yc = cyWnd * 0.5f;
matD2W.set(1.f / w2dx, 0, 0, 1.f / wdy,
centerW.x - xc / w2dx, centerW.y - yc / wdy);
matW2D.set(w2dx, 0, 0, wdy,
xc - w2dx * centerW.x, yc - wdy * centerW.y);
matD2M = matD2W * matW2M;
matM2D = matM2W * matW2D;
}
void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){
///////////////////////////////////
// we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one
CovarianceEstimator ce(_graph);
ce.setVertices(vset);
ce.setGauge(_lastVertex);
ce.compute();
assert(!_lastVertex->fixed() && "last Vertex is fixed");
assert(_firstRobotPose->fixed() && "first Vertex is not fixed");
OptimizableGraph::VertexSet tmpvset = vset;
for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
MatrixXd Pv = ce.getCovariance(vertex);
Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1);
SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate();
Vector2d hxy (delta.translation().x(), delta.translation().y());
double perceptionRange =1;
if (hxy.x()-perceptionRange>0)
hxy.x() -= perceptionRange;
else if (hxy.x()+perceptionRange<0)
hxy.x() += perceptionRange;
else
hxy.x() = 0;
if (hxy.y()-perceptionRange>0)
hxy.y() -= perceptionRange;
else if (hxy.y()+perceptionRange<0)
hxy.y() += perceptionRange;
else
hxy.y() = 0;
double d2 = hxy.transpose() * Pxy.inverse() * hxy;
if (d2 > 5.99)
vset.erase(*it);
}
}
IGL_INLINE void igl::frame_to_cross_field(
const Eigen::MatrixXd& V,
const Eigen::MatrixXi& F,
const Eigen::MatrixXd& FF1,
const Eigen::MatrixXd& FF2,
Eigen::MatrixXd& X)
{
using namespace Eigen;
// Generate local basis
MatrixXd B1, B2, B3;
igl::local_basis(V,F,B1,B2,B3);
// Project the frame fields in the local basis
MatrixXd d1, d2;
d1.resize(F.rows(),2);
d2.resize(F.rows(),2);
d1 << igl::dot_row(B1,FF1), igl::dot_row(B2,FF1);
d2 << igl::dot_row(B1,FF2), igl::dot_row(B2,FF2);
X.resize(F.rows(), 3);
for (int i=0;i<F.rows();i++)
{
Vector2d v1 = d1.row(i);
Vector2d v2 = d2.row(i);
// define inverse map that maps the canonical axis to the given frame directions
Matrix2d A;
A << v1[0], v2[0],
v1[1], v2[1];
// find the closest rotation
Eigen::JacobiSVD<Matrix<double,2,2> > svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
Matrix2d C = svd.matrixU() * svd.matrixV().transpose();
Vector2d v = C.col(0);
X.row(i) = v(0) * B1.row(i) + v(1) * B2.row(i);
}
}
void GiTransform::setModelTransform(const Matrix2d& mat)
{
if (mat.isInvertible() && m_impl->matM2W != mat)
{
m_impl->matM2W = mat;
m_impl->matW2M = m_impl->matM2W.inverse();
m_impl->matD2M = m_impl->matD2W * m_impl->matW2M;
m_impl->matM2D = m_impl->matM2W * m_impl->matW2D;
m_impl->zoomChanged();
}
}
bool GiGraphics::drawPath_(const GiContext* ctx, const MgPath& path, bool fill, const Matrix2d& matD)
{
int n = path.getCount();
if (n == 0 || isStopping())
return false;
const Point2d* pts = path.getPoints();
const char* types = path.getTypes();
Point2d ends, cp1, cp2;
bool matsame = matD.isIdentity();
rawBeginPath();
for (int i = 0; i < n; i++) {
switch (types[i] & ~kMgCloseFigure) {
case kMgMoveTo:
ends = matsame ? pts[i] : (pts[i] * matD);
rawMoveTo(ends.x, ends.y);
break;
case kMgLineTo:
ends = matsame ? pts[i] : (pts[i] * matD);
rawLineTo(ends.x, ends.y);
break;
case kMgBezierTo:
if (i + 2 >= n)
return false;
cp1 = matsame ? pts[i] : (pts[i] * matD);
cp2 = matsame ? pts[i+1] : (pts[i+1] * matD);
ends = matsame ? pts[i+2] : (pts[i+2] * matD);
rawBezierTo(cp1.x, cp1.y, cp2.x, cp2.y, ends.x, ends.y);
i += 2;
break;
case kMgQuadTo:
if (i + 1 >= n)
return false;
cp1 = matsame ? pts[i] : (pts[i] * matD);
ends = matsame ? pts[i+1] : (pts[i+1] * matD);
rawQuadTo(cp1.x, cp1.y, ends.x, ends.y);
i++;
break;
default:
return false;
}
if (types[i] & kMgCloseFigure)
rawClosePath();
}
return rawEndPath(ctx, fill);
}
/**
* This method is used to overload operator* (matrix-matrix multiplication)
* @author Razmik Avetisyan
* @param Object Matrix2d for multiplication
* @return Result of multiplication
*/
Matrix2d Matrix2d::operator* (Matrix2d data)
{
GLdouble *tmp = data.get();
GLdouble temp[3][3];
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
temp[i][j] = (tmp[j] * m[2][i])+ (tmp[1*3 + j] * m[1][i]) + (tmp[2*3 + j] * m[0][i]);
Matrix2d obj(temp);
return obj;
}
void traceback(int x, int y, Matrix2d<char>& sources, string s)
{
printf("x: %i y: %i s: %i\n", x, y, (int)sources.getAt (x,y));
if(x == 0 && y == 0)
{
cout << s << endl;
}
char c = sources.getAt (x,y);
if(insval(c) > 0)
{
traceback(x-1, y, sources, s + seq2.at (y));
}
if(subval(c) > 0)
{
traceback(x-1, y-1, sources, s + "x");
}
if(delval(c) > 0)
{
traceback(x, y-1, sources, s + "-");
}
}
bool GiTransform::setModelTransform(const Matrix2d& mat)
{
bool changed = mat.isInvertible() && m_impl->matM2W != mat;
if (changed) {
m_impl->matM2W = mat;
m_impl->matW2M = m_impl->matM2W.inverse();
m_impl->matD2M = m_impl->matD2W * m_impl->matW2M;
m_impl->matM2D = m_impl->matM2W * m_impl->matW2D;
m_impl->zoomChanged();
}
return changed;
}
Matrix2d Utils::getS(QList<Line*> firstOrtoghonalLines, QList<Line*> secondOrthogonalLines)
{
Matrix2d S;
Vector3d l1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(0));
Vector3d m1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(1));
Vector3d l2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(0));
Vector3d m2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(1));
// A * x = b.
Matrix2d A;
MatrixXd B(2,1);
A << l1(0)*m1(0), l1(0)*m1(1)+l1(1)*m1(0), l2(0)*m2(0), l2(0)*m2(1)+l2(1)*m2(0);
B << -l1(1)*m1(1), -l2(1)*m2(1);
MatrixXd x = A.fullPivLu().solve(B);
S << x(0,0), x(1,0), x(1,0), 1;
return S;
}
int getBestSearchLevel(
const Matrix2d& A_cur_ref,
const int max_level)
{
// Compute patch level in other image
int search_level = 0;
double D = A_cur_ref.determinant();
while(D > 3.0 && search_level < max_level)
{
search_level += 1;
D *= 0.25;
}
return search_level;
}
bool TransformCmd::initialize(const MgMotion* sender)
{
MgCommand* lastCmd = mgGetCommandManager()->getCommand();
if (lastCmd != this) {
_lastCmd = lastCmd;
}
_ptIndex = -1;
if (_axis[0] == _axis[1]) {
_origin = sender->view->xform()->getCenterW();
_axis[0] = Vector2d(40.f, 0);
_axis[1] = Vector2d(0, 40.f);
_xfFirst.setCoordSystem(_axis[0], _axis[1], _origin);
}
sender->view->redraw(true);
return true;
}
void TransformCmd::setPointW(int index, const MgMotion* sender)
{
Point2d point = sender->point * sender->view->xform()->displayToWorld();
if (index > 0) {
float a = (point - _origin).angle2();
float len = _origin.distanceTo(point);
a = floorf(0.5f + a * _M_R2D / 5) * 5 * _M_D2R;
len = floorf(0.5f + len / 5) * 5;
_axis[index == 1 ? 0 : 1].setAngleLength(a, len);
Matrix2d mat(_axis[0], _axis[1], _origin * sender->view->xform()->worldToModel());
mat *= _xfFirst.inverse();
mat = sender->view->shapes()->modelTransform() * mat;
sender->view->xform()->setModelTransform(mat);
sender->view->regen();
} else {
_origin = point;
sender->view->redraw(true);
}
}
void getWarpMatrixAffine(
const vk::AbstractCamera& cam_ref,
const vk::AbstractCamera& cam_cur,
const Vector2d& px_ref,
const Vector3d& f_ref,
const double depth_ref,
const SE3& T_cur_ref,
const int level_ref,
Matrix2d& A_cur_ref)
{
// Compute affine warp matrix A_ref_cur
const int halfpatch_size = 5;
const Vector3d xyz_ref(f_ref*depth_ref);
Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];
xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];
const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));
A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;
}
Matrix2d Matrix2d::scaling(float scaleX, float scaleY, const Point2d& center)
{
Matrix2d mat;
mat.setToScaling(scaleX, scaleY, center);
return mat;
}
Matrix2d Matrix2d::mirroring(const Point2d& pnt, const Vector2d& dir)
{
Matrix2d mat;
mat.setToMirroring(pnt, dir);
return mat;
}
Matrix2d Matrix2d::operator*(const Matrix2d& mat) const
{
Matrix2d result;
result.setToProduct(*this, mat);
return result;
}
Matrix2d Matrix2d::shearing(float sx, float sy, const Point2d& pnt)
{
Matrix2d mat;
mat.setToShearing(sx, sy, pnt);
return mat;
}
Vector2d
FittingCylinder::inverseMapping (const ON_NurbsSurface &nurbs, const Vector3d &pt, const Vector2d &hint, double &error,
Vector3d &p, Vector3d &tu, Vector3d &tv, int maxSteps, double accuracy, bool quiet)
{
double pointAndTangents[9];
Vector2d current, delta;
Matrix2d A;
Vector2d b;
Vector3d r;
std::vector<double> elementsU = getElementVector (nurbs, 0);
std::vector<double> elementsV = getElementVector (nurbs, 1);
double minU = elementsU[0];
double minV = elementsV[0];
double maxU = elementsU[elementsU.size () - 1];
double maxV = elementsV[elementsV.size () - 1];
current = hint;
for (int k = 0; k < maxSteps; k++)
{
nurbs.Evaluate (current (0), current (1), 1, 3, pointAndTangents);
p (0) = pointAndTangents[0];
p (1) = pointAndTangents[1];
p (2) = pointAndTangents[2];
tu (0) = pointAndTangents[3];
tu (1) = pointAndTangents[4];
tu (2) = pointAndTangents[5];
tv (0) = pointAndTangents[6];
tv (1) = pointAndTangents[7];
tv (2) = pointAndTangents[8];
r = p - pt;
b (0) = -r.dot (tu);
b (1) = -r.dot (tv);
A (0, 0) = tu.dot (tu);
A (0, 1) = tu.dot (tv);
A (1, 0) = A (0, 1);
A (1, 1) = tv.dot (tv);
delta = A.ldlt ().solve (b);
if (delta.norm () < accuracy)
{
error = r.norm ();
return current;
}
else
{
current = current + delta;
if (current (0) < minU)
current (0) = minU;
else if (current (0) > maxU)
current (0) = maxU;
if (current (1) < minV)
current (1) = maxV - (minV - current (1));
else if (current (1) > maxV)
current (1) = minV + (current (1) - maxV);
}
}
error = r.norm ();
if (!quiet)
{
printf ("[FittingCylinder::inverseMapping] Warning: Method did not converge (%e %d)\n", accuracy, maxSteps);
printf (" %f %f ... %f %f\n", hint (0), hint (1), current (0), current (1));
}
return current;
}
Matrix2d Matrix2d::mirroring(const Point2d& pnt)
{
Matrix2d mat;
mat.setToMirroring(pnt);
return mat;
}
