int CalcTriangleCenter (const Point3d ** pts, Point3d & c) { static DenseMatrix a(2), inva(2); static Vector rs(2), sol(2); double h = Dist(*pts[0], *pts[1]); Vec3d v1(*pts[0], *pts[1]); Vec3d v2(*pts[0], *pts[2]); rs.Elem(1) = v1 * v1; rs.Elem(2) = v2 * v2; a.Elem(1,1) = 2 * rs.Get(1); a.Elem(1,2) = a.Elem(2,1) = 2 * (v1 * v2); a.Elem(2,2) = 2 * rs.Get(2); if (fabs (a.Det()) <= 1e-12 * h * h) { (*testout) << "CalcTriangleCenter: degenerated" << endl; return 1; } CalcInverse (a, inva); inva.Mult (rs, sol); c = *pts[0]; v1 *= sol.Get(1); v2 *= sol.Get(2); c += v1; c += v2; return 0; }
inline void CalcInverse (const Mat<3,2> & m, Mat<2,3> & inv) { Mat<2,2> a = Trans (m) * m; Mat<2,2> ainv; CalcInverse (a, ainv); inv = ainv * Trans (m); }
// Get result of inverse kinematics (without controller) void UndercarriageCtrlGeom::GetSteerDriveSetValues(std::vector<double> & vdVelGearDriveRadS, std::vector<double> & vdAngGearSteerRad) { //LOG_OUT("Calculate Inverse for given Velocity Command"); CalcInverse(); vdVelGearDriveRadS = m_vdVelGearDriveTarget1RadS; vdAngGearSteerRad = m_vdAngGearSteerTarget1Rad; }
const DenseMatrix &ElementTransformation::EvalInverseJ() { // TODO: compute as invJ = / adjJ/Weight, if J is square, // \ adjJ/Weight^2, otherwise. MFEM_ASSERT((EvalState & INVERSE_MASK) == 0, ""); Jacobian(); invJ.SetSize(dFdx.Width(), dFdx.Height()); if (dFdx.Width() > 0) { CalcInverse(dFdx, invJ); } EvalState |= INVERSE_MASK; return invJ; }
int IsoparametricTransformation::TransformBack(const Vector &pt, IntegrationPoint &ip) { const int max_iter = 16; const double ref_tol = 1e-15; const double phys_tol = 1e-15*pt.Normlinf(); const int dim = FElem->GetDim(); const int sdim = PointMat.Height(); const int geom = FElem->GetGeomType(); IntegrationPoint xip, prev_xip; double xd[3], yd[3], dxd[3], Jid[9]; Vector x(xd, dim), y(yd, sdim), dx(dxd, dim); DenseMatrix Jinv(Jid, dim, sdim); bool hit_bdr = false, prev_hit_bdr; // Use the center of the element as initial guess xip = Geometries.GetCenter(geom); xip.Get(xd, dim); // xip -> x for (int it = 0; it < max_iter; it++) { // Newton iteration: x := x + J(x)^{-1} [pt-F(x)] // or when dim != sdim: x := x + [J^t.J]^{-1}.J^t [pt-F(x)] Transform(xip, y); subtract(pt, y, y); // y = pt-y if (y.Normlinf() < phys_tol) { ip = xip; return 0; } SetIntPoint(&xip); CalcInverse(Jacobian(), Jinv); Jinv.Mult(y, dx); x += dx; prev_xip = xip; prev_hit_bdr = hit_bdr; xip.Set(xd, dim); // x -> xip // If xip is ouside project it on the boundary on the line segment // between prev_xip and xip hit_bdr = !Geometry::ProjectPoint(geom, prev_xip, xip); if (dx.Normlinf() < ref_tol) { ip = xip; return 0; } if (hit_bdr) { xip.Get(xd, dim); // xip -> x if (prev_hit_bdr) { prev_xip.Get(dxd, dim); // prev_xip -> dx subtract(x, dx, dx); // dx = xip - prev_xip if (dx.Normlinf() < ref_tol) { return 1; } } } } ip = xip; return 2; }
void CalcSchurComplement (const FlatMatrix<double> a, FlatMatrix<double> s, const BitArray & used, LocalHeap & lh) { if (s.Height() == 0) return; if (s.Height() == a.Height()) { s = a; return; } HeapReset hr(lh); int n = a.Height(); Array<int> used_dofs(n, lh); Array<int> unused_dofs(n, lh); used_dofs.SetSize(0); unused_dofs.SetSize(0); for (int i = 0; i < n; i++) if (used[i]) used_dofs.Append(i); else unused_dofs.Append(i); s = a.Rows(used_dofs).Cols(used_dofs); FlatMatrix<> b1 = a.Rows(unused_dofs).Cols(used_dofs) | lh; FlatMatrix<> b2 = a.Rows(used_dofs).Cols(unused_dofs) | lh; FlatMatrix<> c = a.Rows(unused_dofs).Cols(unused_dofs) | lh; FlatMatrix<> hb1 (b1.Height(), b1.Width(), lh); if (n > 10) { LapackInverse (c); hb1 = c * b1 | Lapack; s -= b2 * hb1 | Lapack; } else { CalcInverse (c); hb1 = c * b1; s -= b2 * hb1; } }
bool AdFront2 :: Inside (const Point<2> & p) const { int cnt; Vec<2> n; Vec<3> v1; DenseMatrix a(2), ainv(2); Vector b(2), u(2); // quasi-random numbers: n(0) = 0.123871; n(1) = 0.15432; cnt = 0; for (int i = 0; i < lines.Size(); i++) if (lines[i].Valid()) { const Point<3> & p1 = points[lines[i].L().I1()].P(); const Point<3> & p2 = points[lines[i].L().I2()].P(); v1 = p2 - p1; a(0, 0) = v1(0); a(1, 0) = v1(1); a(0, 1) = -n(0); a(1, 1) = -n(1); b(0) = p(0) - p1(0); b(1) = p(1) - p1(1); CalcInverse (a, ainv); ainv.Mult (b, u); if (u(0) >= 0 && u(0) <= 1 && u(1) > 0) cnt++; } return ((cnt % 2) != 0); }
Polyhedra::Face::Face (int pi1, int pi2, int pi3, const ARRAY<Point<3> > & points, int ainputnr) { inputnr = ainputnr; pnums[0] = pi1; pnums[1] = pi2; pnums[2] = pi3; bbox.Set (points[pi1]); bbox.Add (points[pi2]); bbox.Add (points[pi3]); v1 = points[pi2] - points[pi1]; v2 = points[pi3] - points[pi1]; n = Cross (v1, v2); nn = n; nn.Normalize(); // PseudoInverse (v1, v2, w1, w2); Mat<2,3> mat; Mat<3,2> inv; for (int i = 0; i < 3; i++) { mat(0,i) = v1(i); mat(1,i) = v2(i); } CalcInverse (mat, inv); for (int i = 0; i < 3; i++) { w1(i) = inv(i,0); w2(i) = inv(i,1); } }
void Solve (const Vec<H> & rhs, Vec<W> & sol) const { Mat<W,H> inv; CalcInverse (*this, inv); sol = inv * rhs; }
Geometry::Geometry() { // Vertices for Geometry::POINT GeomVert[0] = NULL; // No vertices, dimension is 0 // Vertices for Geometry::SEGMENT GeomVert[1] = new IntegrationRule(2); GeomVert[1]->IntPoint(0).x = 0.0; GeomVert[1]->IntPoint(1).x = 1.0; // Vertices for Geometry::TRIANGLE GeomVert[2] = new IntegrationRule(3); GeomVert[2]->IntPoint(0).x = 0.0; GeomVert[2]->IntPoint(0).y = 0.0; GeomVert[2]->IntPoint(1).x = 1.0; GeomVert[2]->IntPoint(1).y = 0.0; GeomVert[2]->IntPoint(2).x = 0.0; GeomVert[2]->IntPoint(2).y = 1.0; // Vertices for Geometry::SQUARE GeomVert[3] = new IntegrationRule(4); GeomVert[3]->IntPoint(0).x = 0.0; GeomVert[3]->IntPoint(0).y = 0.0; GeomVert[3]->IntPoint(1).x = 1.0; GeomVert[3]->IntPoint(1).y = 0.0; GeomVert[3]->IntPoint(2).x = 1.0; GeomVert[3]->IntPoint(2).y = 1.0; GeomVert[3]->IntPoint(3).x = 0.0; GeomVert[3]->IntPoint(3).y = 1.0; // Vertices for Geometry::TETRAHEDRON GeomVert[4] = new IntegrationRule(4); GeomVert[4]->IntPoint(0).x = 0.0; GeomVert[4]->IntPoint(0).y = 0.0; GeomVert[4]->IntPoint(0).z = 0.0; GeomVert[4]->IntPoint(1).x = 1.0; GeomVert[4]->IntPoint(1).y = 0.0; GeomVert[4]->IntPoint(1).z = 0.0; GeomVert[4]->IntPoint(2).x = 0.0; GeomVert[4]->IntPoint(2).y = 1.0; GeomVert[4]->IntPoint(2).z = 0.0; GeomVert[4]->IntPoint(3).x = 0.0; GeomVert[4]->IntPoint(3).y = 0.0; GeomVert[4]->IntPoint(3).z = 1.0; // Vertices for Geometry::CUBE GeomVert[5] = new IntegrationRule(8); GeomVert[5]->IntPoint(0).x = 0.0; GeomVert[5]->IntPoint(0).y = 0.0; GeomVert[5]->IntPoint(0).z = 0.0; GeomVert[5]->IntPoint(1).x = 1.0; GeomVert[5]->IntPoint(1).y = 0.0; GeomVert[5]->IntPoint(1).z = 0.0; GeomVert[5]->IntPoint(2).x = 1.0; GeomVert[5]->IntPoint(2).y = 1.0; GeomVert[5]->IntPoint(2).z = 0.0; GeomVert[5]->IntPoint(3).x = 0.0; GeomVert[5]->IntPoint(3).y = 1.0; GeomVert[5]->IntPoint(3).z = 0.0; GeomVert[5]->IntPoint(4).x = 0.0; GeomVert[5]->IntPoint(4).y = 0.0; GeomVert[5]->IntPoint(4).z = 1.0; GeomVert[5]->IntPoint(5).x = 1.0; GeomVert[5]->IntPoint(5).y = 0.0; GeomVert[5]->IntPoint(5).z = 1.0; GeomVert[5]->IntPoint(6).x = 1.0; GeomVert[5]->IntPoint(6).y = 1.0; GeomVert[5]->IntPoint(6).z = 1.0; GeomVert[5]->IntPoint(7).x = 0.0; GeomVert[5]->IntPoint(7).y = 1.0; GeomVert[5]->IntPoint(7).z = 1.0; GeomCenter[POINT].x = 0.0; GeomCenter[POINT].y = 0.0; GeomCenter[POINT].z = 0.0; GeomCenter[SEGMENT].x = 0.5; GeomCenter[SEGMENT].y = 0.0; GeomCenter[SEGMENT].z = 0.0; GeomCenter[TRIANGLE].x = 1.0 / 3.0; GeomCenter[TRIANGLE].y = 1.0 / 3.0; GeomCenter[TRIANGLE].z = 0.0; GeomCenter[SQUARE].x = 0.5; GeomCenter[SQUARE].y = 0.5; GeomCenter[SQUARE].z = 0.0; GeomCenter[TETRAHEDRON].x = 0.25; GeomCenter[TETRAHEDRON].y = 0.25; GeomCenter[TETRAHEDRON].z = 0.25; GeomCenter[CUBE].x = 0.5; GeomCenter[CUBE].y = 0.5; GeomCenter[CUBE].z = 0.5; PerfGeomToGeomJac[POINT] = NULL; PerfGeomToGeomJac[SEGMENT] = NULL; PerfGeomToGeomJac[TRIANGLE] = new DenseMatrix(2); PerfGeomToGeomJac[SQUARE] = NULL; PerfGeomToGeomJac[TETRAHEDRON] = new DenseMatrix(3); PerfGeomToGeomJac[CUBE] = NULL; { Linear2DFiniteElement TriFE; IsoparametricTransformation tri_T; tri_T.SetFE(&TriFE); GetPerfPointMat (TRIANGLE, tri_T.GetPointMat()); tri_T.SetIntPoint(&GeomCenter[TRIANGLE]); CalcInverse(tri_T.Jacobian(), *PerfGeomToGeomJac[TRIANGLE]); } { Linear3DFiniteElement TetFE; IsoparametricTransformation tet_T; tet_T.SetFE(&TetFE); GetPerfPointMat (TETRAHEDRON, tet_T.GetPointMat()); tet_T.SetIntPoint(&GeomCenter[TETRAHEDRON]); CalcInverse(tet_T.Jacobian(), *PerfGeomToGeomJac[TETRAHEDRON]); } }
void OCCSurface :: DefineTangentialPlane (const Point<3> & ap1, const PointGeomInfo & geominfo1, const Point<3> & ap2, const PointGeomInfo & geominfo2) { if (projecttype == PLANESPACE) { p1 = ap1; p2 = ap2; //cout << "p1 = " << p1 << endl; //cout << "p2 = " << p2 << endl; GetNormalVector (p1, geominfo1, ez); ex = p2 - p1; ex -= (ex * ez) * ez; ex.Normalize(); ey = Cross (ez, ex); GetNormalVector (p2, geominfo2, n2); nmid = 0.5*(n2+ez); ez = nmid; ez.Normalize(); ex = (p2 - p1).Normalize(); ez -= (ez * ex) * ex; ez.Normalize(); ey = Cross (ez, ex); nmid = ez; //cout << "ex " << ex << " ey " << ey << " ez " << ez << endl; } else { if ( (geominfo1.u < umin) || (geominfo1.u > umax) || (geominfo2.u < umin) || (geominfo2.u > umax) || (geominfo1.v < vmin) || (geominfo1.v > vmax) || (geominfo2.v < vmin) || (geominfo2.v > vmax) ) throw UVBoundsException(); p1 = ap1; p2 = ap2; psp1 = Point<2>(geominfo1.u, geominfo1.v); psp2 = Point<2>(geominfo2.u, geominfo2.v); Vec<3> n; GetNormalVector (p1, geominfo1, n); gp_Pnt pnt; gp_Vec du, dv; occface->D1 (geominfo1.u, geominfo1.v, pnt, du, dv); DenseMatrix D1(3,2), D1T(2,3), DDTinv(2,2); D1(0,0) = du.X(); D1(1,0) = du.Y(); D1(2,0) = du.Z(); D1(0,1) = dv.X(); D1(1,1) = dv.Y(); D1(2,1) = dv.Z(); /* (*testout) << "DefineTangentialPlane" << endl << "---------------------" << endl; (*testout) << "D1 = " << endl << D1 << endl; */ Transpose (D1, D1T); DenseMatrix D1TD1(3,3); D1TD1 = D1T*D1; if (D1TD1.Det() == 0) throw SingularMatrixException(); CalcInverse (D1TD1, DDTinv); DenseMatrix Y(3,2); Vec<3> y1 = (ap2-ap1).Normalize(); Vec<3> y2 = Cross(n, y1).Normalize(); for (int i = 0; i < 3; i++) { Y(i,0) = y1(i); Y(i,1) = y2(i); } DenseMatrix A(2,2); A = DDTinv * D1T * Y; DenseMatrix Ainv(2,2); if (A.Det() == 0) throw SingularMatrixException(); CalcInverse (A, Ainv); for (int i = 0; i < 2; i++) for (int j = 0; j < 2; j++) { Amat(i,j) = A(i,j); Amatinv(i,j) = Ainv(i,j); } Vec<2> temp = Amatinv * (psp2-psp1); double r = temp.Length(); // double alpha = -acos (temp(0)/r); double alpha = -atan2 (temp(1),temp(0)); DenseMatrix R(2,2); R(0,0) = cos (alpha); R(1,0) = -sin (alpha); R(0,1) = sin (alpha); R(1,1) = cos (alpha); A = A*R; if (A.Det() == 0) throw SingularMatrixException(); CalcInverse (A, Ainv); for (int i = 0; i < 2; i++) for (int j = 0; j < 2; j++) { Amat(i,j) = A(i,j); Amatinv(i,j) = Ainv(i,j); } temp = Amatinv * (psp2-psp1); }; }
inline int FindInnerPoint2 (POINTARRAY & points, FACEARRAY & faces, Point3d & p) { static int timer = NgProfiler::CreateTimer ("FindInnerPoint2"); NgProfiler::RegionTimer reg (timer); ARRAY<Vec3d> a; ARRAY<double> c; Mat<3> m, inv; Vec<3> rs, x, pmin; int nf = faces.Size(); a.SetSize (nf); c.SetSize (nf); for (int i = 0; i < nf; i++) { Point3d p1 = points.Get(faces[i][0]); a[i] = Cross (points.Get(faces[i][1]) - p1, points.Get(faces[i][2]) - p1); a[i] /= a[i].Length(); c[i] = - (a[i].X() * p1.X() + a[i].Y() * p1.Y() + a[i].Z() * p1.Z()); } x = 0; double hmax = 0; for (int i = 0; i < nf; i++) { const Element2d & el = faces[i]; for (int j = 1; j <= 3; j++) { double hi = Dist (points.Get(el.PNumMod(j)), points.Get(el.PNumMod(j+1))); if (hi > hmax) hmax = hi; } } double fmin = 0; for (int i1 = 1; i1 <= nf; i1++) for (int i2 = i1+1; i2 <= nf; i2++) for (int i3 = i2+1; i3 <= nf; i3++) for (int i4 = i3+1; i4 <= nf; i4++) { m(0, 0) = a.Get(i1).X() - a.Get(i2).X(); m(0, 1) = a.Get(i1).Y() - a.Get(i2).Y(); m(0, 2) = a.Get(i1).Z() - a.Get(i2).Z(); rs(0) = c.Get(i2) - c.Get(i1); m(1, 0) = a.Get(i1).X() - a.Get(i3).X(); m(1, 1) = a.Get(i1).Y() - a.Get(i3).Y(); m(1, 2) = a.Get(i1).Z() - a.Get(i3).Z(); rs(1) = c.Get(i3) - c.Get(i1); m(2, 0) = a.Get(i1).X() - a.Get(i4).X(); m(2, 1) = a.Get(i1).Y() - a.Get(i4).Y(); m(2, 2) = a.Get(i1).Z() - a.Get(i4).Z(); rs(2) = c.Get(i4) - c.Get(i1); if (fabs (Det (m)) > 1e-10) { CalcInverse (m, inv); x = inv * rs; double f = -1e10; for (int i = 0; i < nf; i++) { double hd = x(0) * a[i].X() + x(1) * a[i].Y() + x(2) * a[i].Z() + c[i]; if (hd > f) f = hd; if (hd > fmin) break; } if (f < fmin) { fmin = f; pmin = x; } } } p = Point3d (pmin(0), pmin(1), pmin(2)); (*testout) << "fmin = " << fmin << endl; return (fmin < -1e-3 * hmax); }
void T_CalcInverse (FlatMatrix<T2> inv) { // static Timer t("CalcInverse"); // RegionTimer reg(t); // Gauss - Jordan - algorithm // Algorithm of Stoer, Einf. i. d. Num. Math, S 145 // int n = m.Height(); int n = inv.Height(); ngstd::ArrayMem<int,100> p(n); // pivot-permutation for (int j = 0; j < n; j++) p[j] = j; for (int j = 0; j < n; j++) { // pivot search double maxval = abs(inv(j,j)); int r = j; for (int i = j+1; i < n; i++) if (abs (inv(j, i)) > maxval) { r = i; maxval = abs (inv(j, i)); } double rest = 0.0; for (int i = j+1; i < n; i++) rest += abs(inv(r, i)); if (maxval < 1e-20*rest) { throw Exception ("Inverse matrix: Matrix singular"); } // exchange rows if (r > j) { for (int k = 0; k < n; k++) swap (inv(k, j), inv(k, r)); swap (p[j], p[r]); } // transformation T2 hr; CalcInverse (inv(j,j), hr); for (int i = 0; i < n; i++) { T2 h = hr * inv(j, i); inv(j, i) = h; } inv(j,j) = hr; for (int k = 0; k < n; k++) if (k != j) { T2 help = inv(n*k+j); T2 h = help * hr; for (int i = 0; i < n; i++) { T2 h = help * inv(n*j+i); inv(n*k+i) -= h; } inv(k,j) = -h; } } // row exchange VectorMem<100,T2> hv(n); for (int i = 0; i < n; i++) { for (int k = 0; k < n; k++) hv(p[k]) = inv(k, i); for (int k = 0; k < n; k++) inv(k, i) = hv(k); } }
bool AdFront2 :: SameSide (const Point<2> & lp1, const Point<2> & lp2, const Array<int> * testfaces) const { int cnt = 0; if (testfaces) { for (int ii = 0; ii < testfaces->Size(); ii++) if (lines[(*testfaces)[ii]].Valid()) { int i = (*testfaces)[ii]; const Point<3> & p13d = points[lines[i].L().I1()].P(); const Point<3> & p23d = points[lines[i].L().I2()].P(); Point<2> p1(p13d(0), p13d(1)); Point<2> p2(p23d(0), p23d(1)); // p1 + alpha v = lp1 + beta vl Vec<2> v = p2-p1; Vec<2> vl = lp2 - lp1; Mat<2,2> mat, inv; Vec<2> rhs, sol; mat(0,0) = v(0); mat(1,0) = v(1); mat(0,1) = -vl(0); mat(1,1) = -vl(1); rhs = lp1-p1; if (Det(mat) == 0) continue; CalcInverse (mat, inv); sol = inv * rhs; if (sol(0) >= 0 && sol(0) <= 1 & sol(1) >= 0 && sol(1) <= 1) { cnt++; } } } else { for (int i = 0; i < lines.Size(); i++) if (lines[i].Valid()) { const Point<3> & p13d = points[lines[i].L().I1()].P(); const Point<3> & p23d = points[lines[i].L().I2()].P(); Point<2> p1(p13d(0), p13d(1)); Point<2> p2(p23d(0), p23d(1)); // p1 + alpha v = lp1 + beta vl Vec<2> v = p2-p1; Vec<2> vl = lp2 - lp1; Mat<2,2> mat, inv; Vec<2> rhs, sol; mat(0,0) = v(0); mat(1,0) = v(1); mat(0,1) = -vl(0); mat(1,1) = -vl(1); rhs = lp1-p1; if (Det(mat) == 0) continue; CalcInverse (mat, inv); sol = inv * rhs; if (sol(0) >= 0 && sol(0) <= 1 & sol(1) >= 0 && sol(1) <= 1) { cnt++; } } } return ((cnt % 2) == 0); }
void Do(LocalHeap & lh) { // We proceed in three steps: // 1. Compute the difference between Q and q // 2. Compute the H(div) Schur complement // 3. Apply Schur complement to the difference // grid function with (interpolated) exact flux, grad(u) BaseVector& vecQ = Q->GetVector(); // numerical flux q BaseVector& vecq = q->GetVector(); // p.w. constant gridfunction to store element-wise error BaseVector& errvec = err->GetVector(); errvec.FV<double>() = 0.0; double sqer =0.0; // this will contain the total error square for(int k=0; k<ma->GetNE(); k++) { ElementId ei (VOL, k); double elndof = ext->GetFE(k,lh).GetNDof(); Vector<SCAL> diff(elndof); // dof nrs: global, global inner, local inner, local Schur Array<int> Gn, Ginn, Linn, Lsn; // compute the difference between Q and q ext->GetDofNrs(k,Gn); // Global# of all dofs on element k diff = SCAL(0.0); for(int j=0; j<elndof; j++) diff[j] = vecQ.FV<SCAL>()[Gn[j]] - vecq.FV<SCAL>()[Gn[j]]; // H(div) Gram matrix (given in two parts in pde file) Matrix<double> elmat(elndof), elmat2(elndof); elmat = 0.0; elmat2 = 0.0; hdivip->GetIntegrator(0)-> CalcElementMatrix(ext->GetFE(ei,lh),ma->GetTrafo(ei,lh),elmat,lh); hdivip->GetIntegrator(1)-> CalcElementMatrix(ext->GetFE(ei,lh),ma->GetTrafo(ei,lh),elmat2,lh); elmat += elmat2; // compute the H(div) Schur complement ext->GetInnerDofNrs(k,Ginn); // Global# of inner dofs on element k for(int j=0; j<elndof; j++) if (Ginn.Contains( Gn[j] )) Linn.Append(j); // Local# of inner dofs on element k else Lsn.Append(j); // Local# of Schur dofs on element k int ielndof = Linn.Size(); Matrix<double> elmati(ielndof),elmatiinv(ielndof); elmati = elmat.Rows(Linn).Cols(Linn); CalcInverse(elmati,elmatiinv); // apply Schur complement to the difference int selndof = elndof - ielndof; Vector<SCAL> diffs(selndof); Matrix<double> S(selndof), Asi(selndof,ielndof); diffs = diff(Lsn); // S = A_ss - A_si * inv(A_ii) * A_is Asi = elmat.Rows(Lsn).Cols(Linn); S = elmat.Rows(Lsn).Cols(Lsn); S -= Asi * elmatiinv * Trans(Asi); // error = (S * diffs, diffs) errvec.FVDouble()[k] = fabs(InnerProduct(diffs, S * diffs)); sqer += errvec.FVDouble()[k]; } cout<<"Discrete H^(-1/2) norm of error in q = "<<sqrt(sqer)<<endl; // write file (don't know what the last argument of AddVariable // does, but 6 seems to be the value everywhere! It seems to intializes // an object of class IM (important message). GetPDE()->AddVariable (string("fluxerr.")+GetName()+".value", sqrt(sqer), 6); }
// Set desired value for Plattfrom Velocity to UndercarriageCtrl (Sollwertvorgabe) void UndercarriageCtrlGeom::SetDesiredPltfVelocity(double dCmdVelLongMMS, double dCmdVelLatMMS, double dCmdRotRobRadS, double dCmdRotVelRadS) { // declare auxiliary variables double dCurrentPosWheelRAD; double dtempDeltaPhi1RAD, dtempDeltaPhi2RAD; // difference between possible steering angels and current steering angle double dtempDeltaPhiCmd1RAD, dtempDeltaPhiCmd2RAD; // difference between possible steering angels and last target steering angle double dtempWeightedDelta1RAD, dtempWeightedDelta2RAD; // weighted Summ of the two distance values // copy function parameters to member variables m_dCmdVelLongMMS = dCmdVelLongMMS; m_dCmdVelLatMMS = dCmdVelLatMMS; m_dCmdRotRobRadS = dCmdRotRobRadS; m_dCmdRotVelRadS = dCmdRotVelRadS; CalcInverse(); // determine optimal Pltf-Configuration for (int i = 0; i<4; i++) { // Normalize Actual Wheel Position before calculation dCurrentPosWheelRAD = m_vdAngGearSteerRad[i]; MathSup::normalizePi(dCurrentPosWheelRAD); // Calculate differences between current config to possible set-points dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - dCurrentPosWheelRAD; dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - dCurrentPosWheelRAD; MathSup::normalizePi(dtempDeltaPhi1RAD); MathSup::normalizePi(dtempDeltaPhi2RAD); // Calculate differences between last steering target to possible set-points dtempDeltaPhiCmd1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerTargetRad[i]; dtempDeltaPhiCmd2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerTargetRad[i]; MathSup::normalizePi(dtempDeltaPhiCmd1RAD); MathSup::normalizePi(dtempDeltaPhiCmd2RAD); // determine optimal setpoint value // 1st which set point is closest to current cinfog // but: avoid permanent switching (if next target is about PI/2 from current config) // 2nd which set point is closest to last set point // "fitness criteria" to choose optimal set point: // calculate accumulted (+ weighted) difference between targets, current config. and last command dtempWeightedDelta1RAD = 0.6*fabs(dtempDeltaPhi1RAD) + 0.4*fabs(dtempDeltaPhiCmd1RAD); dtempWeightedDelta2RAD = 0.6*fabs(dtempDeltaPhi2RAD) + 0.4*fabs(dtempDeltaPhiCmd2RAD); // check which set point "minimizes fitness criteria" if (dtempWeightedDelta1RAD <= dtempWeightedDelta2RAD) { // Target1 is "optimal" m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget1RadS[i]; m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget1Rad[i]; } else { // Target2 is "optimal" m_vdVelGearDriveTargetRadS[i] = m_vdVelGearDriveTarget2RadS[i]; m_vdAngGearSteerTargetRad[i] = m_vdAngGearSteerTarget2Rad[i]; } // provisorial --> skip interpolation and always take Target1 //m_vdVelGearDriveCmdRadS[i] = m_vdVelGearDriveTarget1RadS[i]; //m_vdAngGearSteerCmdRad[i] = m_vdAngGearSteerTarget1Rad[i]; /*// interpolate between last setpoint and theone of the new setpoint, which is closest to the current configuration if (fabs(dtempDeltaPhi1RAD) <= fabs(dtempDeltaPhi2RAD)) { // difference between new target orientation and last (interpolated) target orientation dtempDeltaPhi1RAD = m_vdAngGearSteerTarget1Rad[i] - m_vdAngGearSteerIntpRad[i]; MathSup::normalizePi(dtempDeltaPhi1RAD); // calculate interpolation step sizes, to reach target at end of the cycle m_vdDeltaAngIntpRad[i] = dtempDeltaPhi1RAD; m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget1RadS[i] - m_vdVelGearDriveIntpRadS[i]); // additionally calculate meen change in angular config for feedforward cmd //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi1RAD/m_UnderCarriagePrms.dCmdRateS; } else { // difference between new target orientation and last (interpolated) target orientation dtempDeltaPhi2RAD = m_vdAngGearSteerTarget2Rad[i] - m_vdAngGearSteerIntpRad[i]; MathSup::normalizePi(dtempDeltaPhi2RAD); // calculate interpolation step sizes, to reach target at end of the cycle m_vdDeltaAngIntpRad[i] = dtempDeltaPhi2RAD; m_vdDeltaDriveIntpRadS[i] = (m_vdVelGearDriveTarget2RadS[i] - m_vdVelGearDriveIntpRadS[i]); // additionally calculate meen change in angular config for feedforward cmd //m_vdVelGearSteerIntpRadS[i] = dtempDeltaPhi2RAD/m_UnderCarriagePrms.dCmdRateS; }*/ } /*// Logging for debugging // get current time m_RawTime.SetNow(); m_dNowTime = m_RawTime - m_StartTime; // Log out Pltf-Velocities fprintf(m_pfileDesVel, "%f %f %f %f \n", m_dNowTime, dCmdVelLongMMS, dCmdVelLatMMS, dCmdRotRobRadS); fprintf(m_pfileMeasVel, "%f %f %f %f \n", m_dNowTime, m_dVelLongMMS, m_dVelLatMMS, m_dRotRobRadS); // Log out corresponding Joint-Configuration fprintf(m_pfileSteerAngTarget1, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget1Rad[0], m_vdAngGearSteerTarget1Rad[1], m_vdAngGearSteerTarget1Rad[2], m_vdAngGearSteerTarget1Rad[3]); fprintf(m_pfileSteerAngTarget2, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTarget2Rad[0], m_vdAngGearSteerTarget2Rad[1], m_vdAngGearSteerTarget2Rad[2], m_vdAngGearSteerTarget2Rad[3]); fprintf(m_pfileSteerAngTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdAngGearSteerTargetRad[0], m_vdAngGearSteerTargetRad[1], m_vdAngGearSteerTargetRad[2], m_vdAngGearSteerTargetRad[3]); fprintf(m_pfileDriveVelTarget, "%f %f %f %f %f \n", m_dNowTime, m_vdVelGearDriveTargetRadS[0], m_vdVelGearDriveTargetRadS[1], m_vdVelGearDriveTargetRadS[2], m_vdVelGearDriveTargetRadS[3]);*/ }
int IntersectTriangleLine (const Point<3> ** tri, const Point<3> ** line) { Vec3d vl(*line[0], *line[1]); Vec3d vt1(*tri[0], *tri[1]); Vec3d vt2(*tri[0], *tri[2]); Vec3d vrs(*tri[0], *line[0]); static DenseMatrix a(3), ainv(3); static Vector rs(3), lami(3); int i; /* (*testout) << "Tri-Line inters: " << endl << "tri = " << *tri[0] << ", " << *tri[1] << ", " << *tri[2] << endl << "line = " << *line[0] << ", " << *line[1] << endl; */ for (i = 1; i <= 3; i++) { a.Elem(i, 1) = -vl.X(i); a.Elem(i, 2) = vt1.X(i); a.Elem(i, 3) = vt2.X(i); rs.Elem(i) = vrs.X(i); } double det = a.Det(); double arel = vl.Length() * vt1.Length() * vt2.Length(); /* double amax = 0; for (i = 1; i <= 9; i++) if (fabs (a.Get(i)) > amax) amax = fabs(a.Get(i)); */ // new !!!! if (fabs (det) <= 1e-10 * arel) { #ifdef DEVELOP // line parallel to triangle ! // cout << "ERROR: IntersectTriangleLine degenerated" << endl; // (*testout) << "WARNING: IntersectTriangleLine degenerated\n"; /* (*testout) << "lin-tri intersection: " << endl << "line = " << *line[0] << " - " << *line[1] << endl << "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl << "lami = " << lami << endl << "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl << " = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl << " a = " << a << endl << " ainv = " << ainv << endl << " det(a) = " << det << endl << " rs = " << rs << endl; */ #endif return 0; } CalcInverse (a, ainv); ainv.Mult (rs, lami); // (*testout) << "lami = " << lami << endl; double eps = 1e-6; if ( (lami.Get(1) >= -eps && lami.Get(1) <= 1+eps && lami.Get(2) >= -eps && lami.Get(3) >= -eps && lami.Get(2) + lami.Get(3) <= 1+eps) && ! (lami.Get(1) >= eps && lami.Get(1) <= 1-eps && lami.Get(2) >= eps && lami.Get(3) >= eps && lami.Get(2) + lami.Get(3) <= 1-eps) ) { #ifdef DEVELOP // cout << "WARNING: IntersectTriangleLine degenerated" << endl; (*testout) << "WARNING: IntersectTriangleLine numerical inexact" << endl; (*testout) << "lin-tri intersection: " << endl << "line = " << *line[0] << " - " << *line[1] << endl << "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl << "lami = " << lami << endl << "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl << " = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl << " a = " << a << endl << " ainv = " << ainv << endl << " det(a) = " << det << endl << " rs = " << rs << endl; #endif } if (lami.Get(1) >= 0 && lami.Get(1) <= 1 && lami.Get(2) >= 0 && lami.Get(3) >= 0 && lami.Get(2) + lami.Get(3) <= 1) { return 1; } return 0; }