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;
}
