<commit_before>#include "mfem.hpp"
#include <fstream>
#include <iostream>

using namespace std;
using namespace mfem;

Mesh * GetMesh(int type);

void trans(const Vector &x, Vector &r);

void sigmaFunc(const Vector &x, DenseMatrix &s);

double uExact(const Vector &x)
{
   return (0.25 * (2.0 + x[0]) - x[2]) * (x[2] + 0.25 * (2.0 + x[0]));
}

int main(int argc, char *argv[])
{
   // 1. Parse command-line options.
   int order = 3;
   int mesh_type = 4; // Default to Quadrilateral mesh
   int ref_levels = 0;
   bool static_cond = false;
   bool pa = false;
   const char *device_config = "cpu";
   bool visualization = true;

   OptionsParser args(argc, argv);
   args.AddOption(&mesh_type, "-mt", "--mesh-type",
                  "Mesh type: 3 - Triangular, 4 - Quadrilateral.");
   args.AddOption(&ref_levels, "-r", "--refine",
                  "Number of times to refine the mesh uniformly in serial.");
   args.AddOption(&order, "-o", "--order",
                  "Finite element order (polynomial degree) or -1 for"
                  " isoparametric space.");
   args.AddOption(&static_cond, "-sc", "--static-condensation", "-no-sc",
                  "--no-static-condensation", "Enable static condensation.");
   args.AddOption(&pa, "-pa", "--partial-assembly", "-no-pa",
                  "--no-partial-assembly", "Enable Partial Assembly.");
   args.AddOption(&device_config, "-d", "--device",
                  "Device configuration string, see Device::Configure().");
   args.AddOption(&visualization, "-vis", "--visualization", "-no-vis",
                  "--no-visualization",
                  "Enable or disable GLVis visualization.");
   args.Parse();
   if (!args.Good())
   {
      args.PrintUsage(cout);
      return 1;
   }
   args.PrintOptions(cout);

   // 2. Enable hardware devices such as GPUs, and programming models such as
   //    CUDA, OCCA, RAJA and OpenMP based on command line options.
   Device device(device_config);
   device.Print();

   // 3. Read the mesh from the given mesh file. We can handle triangular,
   //    quadrilateral, tetrahedral, hexahedral, surface and volume meshes with
   //    the same code.
   Mesh *mesh = GetMesh(mesh_type);
   int dim = mesh->Dimension();

   // 4. Refine the mesh to increase the resolution. In this example we do
   //    'ref_levels' of uniform refinement.
   for (int l = 0; l < ref_levels; l++)
   {
      mesh->UniformRefinement();
   }

   mesh->SetCurvature(3);
   mesh->Transform(trans);

   // 5. Define a finite element space on the mesh. Here we use continuous
   //    Lagrange finite elements of the specified order. If order < 1, we
   //    instead use an isoparametric/isogeometric space.
   FiniteElementCollection *fec;
   bool delete_fec;
   if (order > 0)
   {
      fec = new H1_FECollection(order, dim);
      delete_fec = true;
   }
   else if (mesh->GetNodes())
   {
      fec = mesh->GetNodes()->OwnFEC();
      delete_fec = false;
      cout << "Using isoparametric FEs: " << fec->Name() << endl;
   }
   else
   {
      fec = new H1_FECollection(order = 1, dim);
      delete_fec = true;
   }
   FiniteElementSpace fespace(mesh, fec);
   cout << "Number of finite element unknowns: "
        << fespace.GetTrueVSize() << endl;

   // 6. Determine the list of true (i.e. conforming) essential boundary dofs.
   //    In this example, the boundary conditions are defined by marking all
   //    the boundary attributes from the mesh as essential (Dirichlet) and
   //    converting them to a list of true dofs.
   Array<int> ess_tdof_list;
   if (mesh->bdr_attributes.Size())
   {
      Array<int> ess_bdr(mesh->bdr_attributes.Max());
      ess_bdr = 1;
      fespace.GetEssentialTrueDofs(ess_bdr, ess_tdof_list);
   }

   // 7. Set up the linear form b(.) which corresponds to the right-hand side of
   //    the FEM linear system, which in this case is (1,phi_i) where phi_i are
   //    the basis functions in the finite element fespace.
   LinearForm b(&fespace);
   ConstantCoefficient one(1.0);
   b.AddDomainIntegrator(new DomainLFIntegrator(one));
   b.Assemble();

   // 8. Define the solution vector x as a finite element grid function
   //    corresponding to fespace. Initialize x with initial guess of zero,
   //    which satisfies the boundary conditions.
   GridFunction x(&fespace);
   x = 0.0;

   // 9. Set up the bilinear form a(.,.) on the finite element space
   //    corresponding to the Laplacian operator -Delta, by adding the Diffusion
   //    domain integrator.
   BilinearForm a(&fespace);
   MatrixFunctionCoefficient sigma(3, sigmaFunc);
   if (pa) { a.SetAssemblyLevel(AssemblyLevel::PARTIAL); }
   a.AddDomainIntegrator(new DiffusionIntegrator(sigma));

   // 10. Assemble the bilinear form and the corresponding linear system,
   //     applying any necessary transformations such as: eliminating boundary
   //     conditions, applying conforming constraints for non-conforming AMR,
   //     static condensation, etc.
   if (static_cond) { a.EnableStaticCondensation(); }
   a.Assemble();

   OperatorPtr A;
   Vector B, X;
   a.FormLinearSystem(ess_tdof_list, x, b, A, X, B);

   cout << "Size of linear system: " << A->Height() << endl;

   // 11. Solve the linear system A X = B.
   if (!pa)
   {
      // Use a simple symmetric Gauss-Seidel preconditioner with PCG.
      GSSmoother M((SparseMatrix&)(*A));
      PCG(*A, M, B, X, 1, 200, 1e-12, 0.0);
   }
   else // Jacobi preconditioning in partial assembly mode
   {
      if (UsesTensorBasis(fespace))
      {
         OperatorJacobiSmoother M(a, ess_tdof_list);
         PCG(*A, M, B, X, 1, 400, 1e-12, 0.0);
      }
      else
      {
         CG(*A, B, X, 1, 400, 1e-12, 0.0);
      }
   }

   // 12. Recover the solution as a finite element grid function.
   a.RecoverFEMSolution(X, b, x);

   FunctionCoefficient uCoef(uExact);
   double err = x.ComputeL2Error(uCoef);

   mfem::out << "|u - u_h|_2 = " << err << endl;

   // 13. Save the refined mesh and the solution. This output can be viewed later
   //     using GLVis: "glvis -m refined.mesh -g sol.gf".
   ofstream mesh_ofs("refined.mesh");
   mesh_ofs.precision(8);
   mesh->Print(mesh_ofs);
   ofstream sol_ofs("sol.gf");
   sol_ofs.precision(8);
   x.Save(sol_ofs);

   // 14. Send the solution by socket to a GLVis server.
   if (visualization)
   {
      char vishost[] = "localhost";
      int  visport   = 19916;
      socketstream sol_sock(vishost, visport);
      sol_sock.precision(8);
      sol_sock << "solution\n" << *mesh << x << flush;
   }

   // 15. Free the used memory.
   if (delete_fec)
   {
      delete fec;
   }
   delete mesh;

   return 0;
}

Mesh * GetMesh(int type)
{
   Mesh * mesh = NULL;

   if (type == 3)
   {
      mesh = new Mesh(2, 12, 16, 8, 3);

      mesh->AddVertex(-1.0, -1.0, 0.0);
      mesh->AddVertex( 1.0, -1.0, 0.0);
      mesh->AddVertex( 1.0,  1.0, 0.0);
      mesh->AddVertex(-1.0,  1.0, 0.0);
      mesh->AddVertex(-1.0, -1.0, 1.0);
      mesh->AddVertex( 1.0, -1.0, 1.0);
      mesh->AddVertex( 1.0,  1.0, 1.0);
      mesh->AddVertex(-1.0,  1.0, 1.0);
      mesh->AddVertex( 0.0, -1.0, 0.5);
      mesh->AddVertex( 1.0,  0.0, 0.5);
      mesh->AddVertex( 0.0,  1.0, 0.5);
      mesh->AddVertex(-1.0,  0.0, 0.5);

      mesh->AddTriangle(0, 1, 8);
      mesh->AddTriangle(1, 5, 8);
      mesh->AddTriangle(5, 4, 8);
      mesh->AddTriangle(4, 0, 8);
      mesh->AddTriangle(1, 2, 9);
      mesh->AddTriangle(2, 6, 9);
      mesh->AddTriangle(6, 5, 9);
      mesh->AddTriangle(5, 1, 9);
      mesh->AddTriangle(2, 3, 10);
      mesh->AddTriangle(3, 7, 10);
      mesh->AddTriangle(7, 6, 10);
      mesh->AddTriangle(6, 2, 10);
      mesh->AddTriangle(3, 0, 11);
      mesh->AddTriangle(0, 4, 11);
      mesh->AddTriangle(4, 7, 11);
      mesh->AddTriangle(7, 3, 11);

      mesh->AddBdrSegment(0, 1, 1);
      mesh->AddBdrSegment(1, 2, 1);
      mesh->AddBdrSegment(2, 3, 1);
      mesh->AddBdrSegment(3, 0, 1);
      mesh->AddBdrSegment(5, 4, 2);
      mesh->AddBdrSegment(6, 5, 2);
      mesh->AddBdrSegment(7, 6, 2);
      mesh->AddBdrSegment(4, 7, 2);
   }
   else if (type == 4)
   {
      mesh = new Mesh(2, 8, 4, 8, 3);

      mesh->AddVertex(-1.0, -1.0, 0.0);
      mesh->AddVertex( 1.0, -1.0, 0.0);
      mesh->AddVertex( 1.0,  1.0, 0.0);
      mesh->AddVertex(-1.0,  1.0, 0.0);
      mesh->AddVertex(-1.0, -1.0, 1.0);
      mesh->AddVertex( 1.0, -1.0, 1.0);
      mesh->AddVertex( 1.0,  1.0, 1.0);
      mesh->AddVertex(-1.0,  1.0, 1.0);

      mesh->AddQuad(0, 1, 5, 4);
      mesh->AddQuad(1, 2, 6, 5);
      mesh->AddQuad(2, 3, 7, 6);
      mesh->AddQuad(3, 0, 4, 7);

      mesh->AddBdrSegment(0, 1, 1);
      mesh->AddBdrSegment(1, 2, 1);
      mesh->AddBdrSegment(2, 3, 1);
      mesh->AddBdrSegment(3, 0, 1);
      mesh->AddBdrSegment(5, 4, 2);
      mesh->AddBdrSegment(6, 5, 2);
      mesh->AddBdrSegment(7, 6, 2);
      mesh->AddBdrSegment(4, 7, 2);
   }
   else
   {
      MFEM_ABORT("Unrecognized mesh type " << type << "!");
   }
   mesh->FinalizeTopology();

   return mesh;
}

void trans(const Vector &x, Vector &r)
{
   r.SetSize(3);

   double tol = 1e-6;
   double theta = 0.0;
   if (fabs(x[1] + 1.0) < tol)
   {
      theta = 0.25 * M_PI * (x[0] - 2.0);
   }
   else if (fabs(x[0] - 1.0) < tol)
   {
      theta = 0.25 * M_PI * x[1];
   }
   else if (fabs(x[1] - 1.0) < tol)
   {
      theta = 0.25 * M_PI * (2.0 - x[0]);
   }
   else if (fabs(x[0] + 1.0) < tol)
   {
      theta = 0.25 * M_PI * (4.0 - x[1]);
   }
   else
   {
      cout << "side not recognized "
           << x[0] << " " << x[1] << " " << x[2] << endl;
   }

   r[0] = cos(theta);
   r[1] = sin(theta);
   r[2] = 0.25 * (2.0 * x[2] - 1.0) * (r[0] + 2.0);
}

void sigmaFunc(const Vector &x, DenseMatrix &s)
{
   s.SetSize(3);
   double a = 17.0 - 2.0 * x[0] * (1.0 + x[0]);
   s(0,0) = 0.5 + x[0] * x[0] * (8.0 / a - 0.5);
   s(0,1) = x[0] * x[1] * (8.0 / a - 0.5);
   s(0,2) = 0.0;
   s(1,0) = s(0,1);
   s(1,1) = 0.5 * x[0] * x[0] + 8.0 * x[1] * x[1] / a;
   s(1,2) = 0.0;
   s(2,0) = 0.0;
   s(2,1) = 0.0;
   s(2,2) = a / 32.0;
}
<commit_msg>Removing temporary example<commit_after><|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: metric.cxx,v $
 *
 *  $Revision: 1.2 $
 *
 *  last change: $Author: hdu $ $Date: 2001-06-28 15:08:05 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library 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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#include <metric.hxx>

// =======================================================================

FontInfo::FontInfo()
{
    mpImplMetric                = new ImplFontMetric;
    mpImplMetric->mnRefCount    = 1;
    mpImplMetric->meType        = TYPE_DONTKNOW;
    mpImplMetric->mbDevice      = FALSE;
    mpImplMetric->mnAscent      = 0;
    mpImplMetric->mnDescent     = 0;
    mpImplMetric->mnLeading     = 0;
    mpImplMetric->mnLineHeight  = 0;
    mpImplMetric->mnSlant       = 0;
    mpImplMetric->mnFirstChar   = 0;
    mpImplMetric->mnLastChar    = 0;
}

// -----------------------------------------------------------------------

FontInfo::FontInfo( const FontInfo& rInfo ) :
    Font( rInfo )
{
    mpImplMetric = rInfo.mpImplMetric;
    mpImplMetric->mnRefCount++;
}

// -----------------------------------------------------------------------

FontInfo::~FontInfo()
{
    // Eventuell Metric loeschen
    if ( mpImplMetric->mnRefCount > 1 )
        mpImplMetric->mnRefCount--;
    else
        delete mpImplMetric;
}

// -----------------------------------------------------------------------

FontInfo& FontInfo::operator=( const FontInfo& rInfo )
{
    Font::operator=( rInfo );

    // Zuerst Referenzcounter erhoehen, damit man sich selbst zuweisen kann
    rInfo.mpImplMetric->mnRefCount++;

    // Sind wir nicht die letzten ?
    if ( mpImplMetric->mnRefCount > 1 )
        mpImplMetric->mnRefCount--;
    else
        delete mpImplMetric;

    mpImplMetric = rInfo.mpImplMetric;

    return *this;

}

// -----------------------------------------------------------------------

BOOL FontInfo::operator==( const FontInfo& rInfo ) const
{
    if ( !Font::operator==( rInfo ) )
        return FALSE;

    if ( mpImplMetric == rInfo.mpImplMetric )
        return TRUE;

    if ( (mpImplMetric->meType      == rInfo.mpImplMetric->meType       ) &&
         (mpImplMetric->mbDevice    == rInfo.mpImplMetric->mbDevice     ) &&
         (mpImplMetric->mnAscent    == rInfo.mpImplMetric->mnAscent     ) &&
         (mpImplMetric->mnDescent   == rInfo.mpImplMetric->mnDescent    ) &&
         (mpImplMetric->mnLeading   == rInfo.mpImplMetric->mnLeading    ) &&
         (mpImplMetric->mnSlant     == rInfo.mpImplMetric->mnSlant      ) &&
         (mpImplMetric->mnFirstChar == rInfo.mpImplMetric->mnFirstChar  ) &&
         (mpImplMetric->mnLastChar  == rInfo.mpImplMetric->mnLastChar   ) )
        return TRUE;
    else
        return FALSE;
}

// =======================================================================

static const sal_UCS4 pDefaultRangeCodes[] = {0x0020,0xD800, 0xE000,0xFFF0};

FontCharMap::FontCharMap()
{
    ImplSetDefaultRanges();
}

// -----------------------------------------------------------------------

FontCharMap::~FontCharMap()
{
    if( mpRangeCodes && mpRangeCodes != pDefaultRangeCodes )
        delete[] mpRangeCodes;
}

// -----------------------------------------------------------------------

void FontCharMap::ImplSetRanges( ULONG nPairs, const sal_UCS4* pCodes )
{
    mnRangeCount = nPairs;
    mpRangeCodes = pCodes;

    mnCharCount = 0;
    for( int i = 0; i < nPairs; ++i )
        mnCharCount += mpRangeCodes[ 2*i+1 ] - mpRangeCodes[ 2*i ];
}

// -----------------------------------------------------------------------

void FontCharMap::ImplSetDefaultRanges()
{
    int nCodes = sizeof(pDefaultRangeCodes)/sizeof(*pDefaultRangeCodes);
    ImplSetRanges( nCodes, pDefaultRangeCodes );
}

// -----------------------------------------------------------------------

BOOL FontCharMap::IsDefaultMap() const
{
    return (mpRangeCodes == pDefaultRangeCodes);
}

// -----------------------------------------------------------------------

int FontCharMap::ImplFindRange( sal_UCS4 cChar ) const
{
    int nLower = 0;
    int nMid   = mnRangeCount;
    int nUpper = 2 * mnRangeCount;
    while( nLower < nUpper ) {
        if( cChar >= mpRangeCodes[ nMid ] )
            nLower = nMid;
        else
            nUpper = nMid - 1;
        nMid = (nLower + nUpper + 1) / 2;
    }
    return nMid;
}

// -----------------------------------------------------------------------

BOOL FontCharMap::HasChar( sal_UCS4 cChar ) const
{
    int nRange = ImplFindRange( cChar );
    if( nRange==0 && cChar<mpRangeCodes[0] )
        return FALSE;
    return (nRange & 1) ? FALSE: TRUE;
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetFirstChar() const
{
    return mpRangeCodes[0];
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetLastChar() const
{
    return (mpRangeCodes[ 2*mnRangeCount-1 ] - 1);
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetNextChar( sal_UCS4 cChar ) const
{
    if( cChar < GetFirstChar() )
        return GetFirstChar();
    if( cChar >= GetLastChar() )
        return GetLastChar();

    int nRange = ImplFindRange( cChar );
    if( nRange & 1 )                        // inbetween ranges?
        return mpRangeCodes[ nRange + 1 ];  // first in next range
    return (cChar + 1);
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetPrevChar( sal_UCS4 cChar ) const
{
    if( cChar <= GetFirstChar() )
        return GetFirstChar();
    if( cChar > GetLastChar() )
        return GetLastChar();

    int nRange = ImplFindRange( cChar );
    // inbetween ranges or first in range?
    if( (nRange & 1) || (cChar == mpRangeCodes[ nRange ]) )
        return (mpRangeCodes[ nRange ] - 1);   // last in prev range
    return (cChar - 1);
}

// =======================================================================
<commit_msg>#65293# circumvent MSVC++ bug<commit_after>/*************************************************************************
 *
 *  $RCSfile: metric.cxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: hdu $ $Date: 2001-07-03 09:30:42 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library 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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#include <metric.hxx>

// =======================================================================

FontInfo::FontInfo()
{
    mpImplMetric                = new ImplFontMetric;
    mpImplMetric->mnRefCount    = 1;
    mpImplMetric->meType        = TYPE_DONTKNOW;
    mpImplMetric->mbDevice      = FALSE;
    mpImplMetric->mnAscent      = 0;
    mpImplMetric->mnDescent     = 0;
    mpImplMetric->mnLeading     = 0;
    mpImplMetric->mnLineHeight  = 0;
    mpImplMetric->mnSlant       = 0;
    mpImplMetric->mnFirstChar   = 0;
    mpImplMetric->mnLastChar    = 0;
}

// -----------------------------------------------------------------------

FontInfo::FontInfo( const FontInfo& rInfo ) :
    Font( rInfo )
{
    mpImplMetric = rInfo.mpImplMetric;
    mpImplMetric->mnRefCount++;
}

// -----------------------------------------------------------------------

FontInfo::~FontInfo()
{
    // Eventuell Metric loeschen
    if ( mpImplMetric->mnRefCount > 1 )
        mpImplMetric->mnRefCount--;
    else
        delete mpImplMetric;
}

// -----------------------------------------------------------------------

FontInfo& FontInfo::operator=( const FontInfo& rInfo )
{
    Font::operator=( rInfo );

    // Zuerst Referenzcounter erhoehen, damit man sich selbst zuweisen kann
    rInfo.mpImplMetric->mnRefCount++;

    // Sind wir nicht die letzten ?
    if ( mpImplMetric->mnRefCount > 1 )
        mpImplMetric->mnRefCount--;
    else
        delete mpImplMetric;

    mpImplMetric = rInfo.mpImplMetric;

    return *this;

}

// -----------------------------------------------------------------------

BOOL FontInfo::operator==( const FontInfo& rInfo ) const
{
    if ( !Font::operator==( rInfo ) )
        return FALSE;

    if ( mpImplMetric == rInfo.mpImplMetric )
        return TRUE;

    if ( (mpImplMetric->meType      == rInfo.mpImplMetric->meType       ) &&
         (mpImplMetric->mbDevice    == rInfo.mpImplMetric->mbDevice     ) &&
         (mpImplMetric->mnAscent    == rInfo.mpImplMetric->mnAscent     ) &&
         (mpImplMetric->mnDescent   == rInfo.mpImplMetric->mnDescent    ) &&
         (mpImplMetric->mnLeading   == rInfo.mpImplMetric->mnLeading    ) &&
         (mpImplMetric->mnSlant     == rInfo.mpImplMetric->mnSlant      ) &&
         (mpImplMetric->mnFirstChar == rInfo.mpImplMetric->mnFirstChar  ) &&
         (mpImplMetric->mnLastChar  == rInfo.mpImplMetric->mnLastChar   ) )
        return TRUE;
    else
        return FALSE;
}

// =======================================================================

static const sal_UCS4 pDefaultRangeCodes[] = {0x0020,0xD800, 0xE000,0xFFF0};

FontCharMap::FontCharMap()
{
    ImplSetDefaultRanges();
}

// -----------------------------------------------------------------------

FontCharMap::~FontCharMap()
{
    ImplSetRanges( 0, NULL );
}

// -----------------------------------------------------------------------

void FontCharMap::ImplSetRanges( ULONG nPairs, const sal_UCS4* pCodes )
{
    if( mpRangeCodes && mpRangeCodes != pDefaultRangeCodes )
        delete[] const_cast<ULONG*>( mpRangeCodes );

    mnRangeCount = nPairs;
    mpRangeCodes = pCodes;

    mnCharCount = 0;
    for( int i = 0; i < nPairs; ++i )
        mnCharCount += mpRangeCodes[ 2*i+1 ] - mpRangeCodes[ 2*i ];
}

// -----------------------------------------------------------------------

void FontCharMap::ImplSetDefaultRanges()
{
    int nCodes = sizeof(pDefaultRangeCodes)/sizeof(*pDefaultRangeCodes);
    ImplSetRanges( nCodes, pDefaultRangeCodes );
}

// -----------------------------------------------------------------------

BOOL FontCharMap::IsDefaultMap() const
{
    return (mpRangeCodes == pDefaultRangeCodes);
}

// -----------------------------------------------------------------------

int FontCharMap::ImplFindRange( sal_UCS4 cChar ) const
{
    int nLower = 0;
    int nMid   = mnRangeCount;
    int nUpper = 2 * mnRangeCount;
    while( nLower < nUpper ) {
        if( cChar >= mpRangeCodes[ nMid ] )
            nLower = nMid;
        else
            nUpper = nMid - 1;
        nMid = (nLower + nUpper + 1) / 2;
    }
    return nMid;
}

// -----------------------------------------------------------------------

BOOL FontCharMap::HasChar( sal_UCS4 cChar ) const
{
    int nRange = ImplFindRange( cChar );
    if( nRange==0 && cChar<mpRangeCodes[0] )
        return FALSE;
    return (nRange & 1) ? FALSE: TRUE;
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetFirstChar() const
{
    return mpRangeCodes[0];
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetLastChar() const
{
    return (mpRangeCodes[ 2*mnRangeCount-1 ] - 1);
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetNextChar( sal_UCS4 cChar ) const
{
    if( cChar < GetFirstChar() )
        return GetFirstChar();
    if( cChar >= GetLastChar() )
        return GetLastChar();

    int nRange = ImplFindRange( cChar );
    if( nRange & 1 )                        // inbetween ranges?
        return mpRangeCodes[ nRange + 1 ];  // first in next range
    return (cChar + 1);
}

// -----------------------------------------------------------------------

sal_UCS4 FontCharMap::GetPrevChar( sal_UCS4 cChar ) const
{
    if( cChar <= GetFirstChar() )
        return GetFirstChar();
    if( cChar > GetLastChar() )
        return GetLastChar();

    int nRange = ImplFindRange( cChar );
    // inbetween ranges or first in range?
    if( (nRange & 1) || (cChar == mpRangeCodes[ nRange ]) )
        return (mpRangeCodes[ nRange ] - 1);   // last in prev range
    return (cChar - 1);
}

// =======================================================================
<|endoftext|>


<commit_before>
// Initialization

#include "getfem/getfem_model_solvers.h"
#include "getfem/getfem_export.h"
#include "gmm/gmm.h"
#include "getfem/getfem_mesher.h"
#include "getfem/getfem_generic_assembly.h"

using bgeot::size_type;
using bgeot::base_node;
using bgeot::base_small_vector;
typedef getfem::model_real_plain_vector plain_vector;

int main(void) {

// Parameters of the model

  double epsilon = 1.; // Thickness of the plate (cm)
  double E = 21E6;     // Young Modulus (N/cm^2)
  double nu = 0.3;     // Poisson ratio
  double clambda = E*nu/((1+nu)*(1-2*nu));
  double cmu = E/(2*(1+nu));
  double clambdastar = 2*clambda*cmu/(clambda+2*cmu);
  double F = 100E2;    // Force density at the right boundary (N/cm^2)
  double kappa = 4.;   // Thermal conductivity (W/(cm K))
  double D = 10.;      // Heat transfert coefficient (W/(K cm^2))
  double air_temp = 20;// Temperature of the air in oC.
  double alpha_th = 16.6E-6; // Thermal expansion coefficient (/K)
  double T0 = 20.;     // Reference temperature in oC
  double rho_0 = 1.754E-8; // Resistance temperature coefficient at T0
  double alpha = 0.0039; // Second resistance temperature coefficient

  double h = 2;        // Approximate mesh size
  bgeot::dim_type elements_degree = 2; // Degree of the finite element methods

// Mesh generation

  getfem::mesh mesh;
  getfem::mesher_rectangle mo1(base_node(0., 0.), base_node(100., 25.));
  getfem::mesher_ball mo2(base_node(25., 12.5), 8.);
  getfem::mesher_ball mo3(base_node(50., 12.5), 8.);
  getfem::mesher_ball mo4(base_node(75., 12.5), 8.);
  getfem::mesher_union mo5(mo2, mo3, mo4);
  getfem::mesher_setminus mo(mo1, mo5);

  std::vector<getfem::base_node> fixed;
  getfem::build_mesh(mesh, mo, h, fixed, 2, -2);

// Mesh draw

  getfem::vtk_export exp("mesh.vtk", false);
  exp.exporting(mesh);
  exp.write_mesh();
  // You can view the mesh for instance with
  // mayavi2 -d mesh.vtk -f ExtractEdges -m Surface
}
<commit_msg>Definition of finite element methods and integration methodを追加<commit_after>
// Initialization

#include <iostream>
#include "getfem/getfem_model_solvers.h"
#include "getfem/getfem_export.h"
#include "gmm/gmm.h"
#include "getfem/getfem_mesher.h"
#include "getfem/getfem_generic_assembly.h"

using namespace std;
using bgeot::size_type;
using bgeot::base_node;
using bgeot::base_small_vector;
typedef getfem::model_real_plain_vector plain_vector;

int main(void) {

// Parameters of the model

  double epsilon = 1.; // Thickness of the plate (cm)
  double E = 21E6;     // Young Modulus (N/cm^2)
  double nu = 0.3;     // Poisson ratio
  double clambda = E*nu/((1+nu)*(1-2*nu));
  double cmu = E/(2*(1+nu));
  double clambdastar = 2*clambda*cmu/(clambda+2*cmu);
  double F = 100E2;    // Force density at the right boundary (N/cm^2)
  double kappa = 4.;   // Thermal conductivity (W/(cm K))
  double D = 10.;      // Heat transfert coefficient (W/(K cm^2))
  double air_temp = 20;// Temperature of the air in oC.
  double alpha_th = 16.6E-6; // Thermal expansion coefficient (/K)
  double T0 = 20.;     // Reference temperature in oC
  double rho_0 = 1.754E-8; // Resistance temperature coefficient at T0
  double alpha = 0.0039; // Second resistance temperature coefficient

  double h = 2;        // Approximate mesh size
  bgeot::dim_type elements_degree = 2; // Degree of the finite element methods

// Mesh generation

  getfem::mesh mesh;
  getfem::mesher_rectangle mo1(base_node(0., 0.), base_node(100., 25.));
  getfem::mesher_ball mo2(base_node(25., 12.5), 8.);
  getfem::mesher_ball mo3(base_node(50., 12.5), 8.);
  getfem::mesher_ball mo4(base_node(75., 12.5), 8.);
  getfem::mesher_union mo5(mo2, mo3, mo4);
  getfem::mesher_setminus mo(mo1, mo5);

  std::vector<getfem::base_node> fixed;
  getfem::build_mesh(mesh, mo, h, fixed, 2, -2);

// Mesh draw

  getfem::vtk_export exp("mesh.vtk", false);
  exp.exporting(mesh);
  exp.write_mesh();
  // You can view the mesh for instance with
  // mayavi2 -d mesh.vtk -f ExtractEdges -m Surface

// Definition of finite element methods and integration method

  getfem::mesh_fem mfu(mesh, 2);
  mfu.set_classical_finite_element(elements_degree);
  getfem::mesh_fem mft(mesh, 1);
  mft.set_classical_finite_element(elements_degree);
  getfem::mesh_fem mfvm(mesh, 1);
  mfvm.set_classical_discontinuous_finite_element(elements_degree);

  getfem::mesh_im  mim(mesh);
  mim.set_integration_method(bgeot::dim_type(gmm::sqr(elements_degree)));

}
<|endoftext|>


<commit_before>/***************************************************************************
**
** Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (directui@nokia.com)
**
** This file is part of libmeegotouch.
**
** If you have questions regarding the use of this file, please contact
** Nokia at directui@nokia.com.
**
** This library is free software; you can redistribute it and/or
** modify it under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation
** and appearing in the file LICENSE.LGPL included in the packaging
** of this file.
**
****************************************************************************/

#include "muniquestringcache_p.h"
#include "muniquestringcache.h"

#include "mdebug.h"
#include "mcomponentdata.h"

#include <QFile>
#include <QFileInfo>
#include <QDir>
#include <QSystemSemaphore>
#include <QHash>
#include <QByteArray>
#include <QApplication>
#include <QScopedPointer>

namespace {
const int MAX_CACHE_SIZE = 1024*1024;
#define CACHE_NAME "MTF_UNIQUE_STRING_CACHE"

// slightly adapted from the version found in Qt's corelib/tools/qhash.cpp
static uint hash(const uchar *p)
{
    uint h = 0;

    while (*p) {
        h = (h << 4) + *p++;
        h ^= (h & 0xf0000000) >> 23;
        h &= 0x0fffffff;
    }
    return h;
}

}

uint qHash(const QLatin1String &key)
{
    return hash(reinterpret_cast<const uchar *>(key.latin1()));
}

class UniqueStringCacheMappedMemory
{
public:
    UniqueStringCacheMappedMemory(const QString& filename)
        : cacheFile(filename),
        accessSemaphore(filename + "_UNIQUE_STRING_SEMAPHORE", 1),
        rawMappedMemory(0),
        attached(false),
        locked(false)
    {
        if (!accessSemaphore.acquire()) {
            mWarning("UniqueStringCacheMappedMemory::UniqueStringCacheMappedMemory") << "Unable to aquire semaphore:" << accessSemaphore.errorString();
            return;
        }

        attachToCache();

        accessSemaphore.release();
    }

    bool isAttached() const { return attached; }

    uchar* data() { return rawMappedMemory; }
    QString errorString() { return accessSemaphore.errorString(); }

    ~UniqueStringCacheMappedMemory()
    {
    }

    bool isLocked()
    {
        return locked;
    }

    bool lock() {
        bool result = accessSemaphore.acquire();
        locked = result;
        return result;

    }
    bool unlock() {
        locked = false;
        return accessSemaphore.release();
    }

private:
    friend class MUniqueStringCacheLocker;

    void attachToCache()
    {
        if (!cacheFile.exists()) {
            if (!initializeCache()) {
                attached = false;
                return;
            }
        }

        if (!cacheFile.open(QIODevice::ReadWrite)) {
            mWarning("UniqueStringCacheMappedMemory") << "Could not open" <<
                cacheFile.fileName();
            return;
        }

        if (cacheFile.size() != MAX_CACHE_SIZE) {
            mWarning("UniqueStringCacheMappedMemory") << "Wrong cache file size" <<
                cacheFile.size() << "Expected:" << MAX_CACHE_SIZE;
            return;
        }

        rawMappedMemory = cacheFile.map(0, MAX_CACHE_SIZE);

        attached = true;

        cacheFile.close();
    }

    bool initializeCache() {
        if (!cacheFile.open(QFile::WriteOnly)) {
            //Maybe it failed because the directory doesn't exist
            QDir().mkpath(QFileInfo(cacheFile.fileName()).absolutePath());
            if (!cacheFile.open(QFile::WriteOnly)) {
                mWarning("UniqueStringCacheMappedMemory") <<
                         "Wrong permissions for cache directory. Cannot create" <<
                          cacheFile.fileName();
                return false;
            }
        }
        cacheFile.resize(MAX_CACHE_SIZE);
        cacheFile.close();
        return true;
    }

    QFile cacheFile;
    QSystemSemaphore accessSemaphore;
    uchar* rawMappedMemory;
    bool attached;
    bool locked;
};

MUniqueStringCachePrivate::MUniqueStringCachePrivate(MUniqueStringCache *parent, const QString &filename)
    : q_ptr(parent),
    filename(filename),
    uniqueStringCacheMappedMemory(new UniqueStringCacheMappedMemory(filename)),
    offset(sizeof(int)),
    cacheFilled(false),
    stringToIdCacheFilled(false)
{
}

MUniqueStringCachePrivate::~MUniqueStringCachePrivate()
{
    delete uniqueStringCacheMappedMemory;
}

// fills our datastructures from memory
// this method can be called several times to update the internal representation
// if the cache has been changed by another application
void MUniqueStringCachePrivate::fillUniqueStringCache() {
    uchar *rawCache = uniqueStringCacheMappedMemory->data();
    int elementsInCache = *reinterpret_cast<int*>(rawCache);
    int oldSize = idToPointerCache.count();
    idToPointerCache.resize(elementsInCache);

    for (int i = oldSize; i < elementsInCache; ++i) {
        uchar *stringAdress = rawCache + offset;
        int stringLength = *reinterpret_cast<int*>(stringAdress);

        idToPointerCache[i] = stringAdress;

        offset += sizeof(int) + stringLength + 1;

        if (stringToIdCacheFilled) {
            stringToIdCache.insert(QLatin1String(q_ptr->indexToString(i)), i);
        }
    }
    if (elementsInCache == 0) {
        // we want the empty string as first element
        insertStringToCache(QByteArray());
    }

    mDebug("MUniqueStringCachePrivate::fillUniqueStringCache")
            << QString("Elements in cache %1: %2, %3% filled").arg(filename).arg(elementsInCache).arg((float)offset/MAX_CACHE_SIZE*100);
}

// insert one string into the cache, makes sure to handle it gracefully when the cache has
// changed before it has been locked
int MUniqueStringCachePrivate::insertStringToCache(const QByteArray &string) {
    uchar *rawCache = uniqueStringCacheMappedMemory->data();
    int elementsInCache = *reinterpret_cast<int*>(rawCache);
    if (elementsInCache != idToPointerCache.count()) {
        // cache has changed, check if the string has been added
        // in the meanwhile
        fillUniqueStringCache();
        if (!stringToIdCacheFilled) {
            fillStringToIdCache();
        }
        StringToIdCache::const_iterator it = stringToIdCache.constFind(QLatin1String(string.constData()));
        if (it != stringToIdCache.constEnd()) {
            return *it;
        }
    }

    ++*reinterpret_cast<int*>(rawCache);

    uchar *stringAdress = rawCache + offset;
    *reinterpret_cast<int*>(stringAdress) = string.length();
    memcpy(stringAdress + sizeof(int), string.constData(), string.length() + 1);

    offset += sizeof(int) + string.length() + 1;
    if (offset >= MAX_CACHE_SIZE) {
        qFatal("unique string cache is full");
    }

    idToPointerCache.append((uchar*)stringAdress);
    stringToIdCache.insert(QLatin1String(reinterpret_cast<const char*>(stringAdress + sizeof(int))), idToPointerCache.count() - 1);

    return idToPointerCache.count() - 1;
}

// triggers initialization of our data structures from the cache if this
// did not happen yet
void MUniqueStringCachePrivate::initiallyFillCache() {
    if (!cacheFilled) {
        QScopedPointer<MUniqueStringCacheLocker> locker;
        if (!q_ptr->isLocked()) {
            locker.reset(new MUniqueStringCacheLocker(q_ptr));
            if (!locker->isLocked()) {
                return;
            }
        }
        fillUniqueStringCache();
        cacheFilled = true;
    }
}

void MUniqueStringCachePrivate::fillStringToIdCache()
{
    mDebug("MUniqueStringCachePrivate::fillStringToIdCache") << "filling stringToIdCache for" << filename;
    for (int i = 0; i < idToPointerCache.count(); ++i) {
        stringToIdCache.insert(QLatin1String(q_ptr->indexToString(i)), i);
    }
    stringToIdCacheFilled = true;
}

//////////////////////////////////////

MUniqueStringCache::MUniqueStringCache(const QString& filename)
    : d_ptr(new MUniqueStringCachePrivate(this, filename))
{
}

MUniqueStringCache::~MUniqueStringCache()
{
    delete d_ptr;
}

bool MUniqueStringCache::isAttached()
{
    return d_ptr->uniqueStringCacheMappedMemory->isAttached();
}

MUniqueStringCache::Index MUniqueStringCache::stringToIndex(const QByteArray& string)
{
    if (string.isEmpty()) {
        return EmptyStringIndex;
    }

    d_ptr->initiallyFillCache();

    if (!d_ptr->stringToIdCacheFilled) {
        d_ptr->fillStringToIdCache();
    }

    MUniqueStringCachePrivate::StringToIdCache::const_iterator it = d_ptr->stringToIdCache.constFind(QLatin1String(string.constData()));
    if (it != d_ptr->stringToIdCache.constEnd()) {
        return *it;
    }

    // string is unknown, we need to add it to the cache
    QScopedPointer<MUniqueStringCacheLocker> locker;
    if (!isLocked()) {
        locker.reset(new MUniqueStringCacheLocker(this));
        if (!locker->isLocked()) {
            return UndefinedIndex;
        }
    }
    return d_ptr->insertStringToCache(string);
}

QLatin1String MUniqueStringCache::indexToString(Index id)
{
    d_ptr->initiallyFillCache();
    if (id < 0) {
        return indexToString(EmptyStringIndex);
    } else if (id >= d_ptr->idToPointerCache.count())  {
        // it may be a new string in the cache. try to update the cache and then check again
        QScopedPointer<MUniqueStringCacheLocker> locker;
        if (!isLocked()) {
            locker.reset(new MUniqueStringCacheLocker(this));
            if (!locker->isLocked()) {
                return indexToString(EmptyStringIndex);
            }
        }
        d_ptr->fillUniqueStringCache();
        if (id >= d_ptr->idToPointerCache.count()) {
            mWarning("MUniqueStringCache::indexToString") << "Id" << id << "is unknown. Has the string cache been deleted?" << d_ptr->idToPointerCache.count();
            return indexToString(EmptyStringIndex);
        }
    }

    uchar *stringAdress = d_ptr->idToPointerCache.at(id);
    return QLatin1String(reinterpret_cast<const char*>(stringAdress + sizeof(int)));
}

bool MUniqueStringCache::lock()
{
    return d_ptr->uniqueStringCacheMappedMemory->lock();
}

bool MUniqueStringCache::unlock()
{
    return d_ptr->uniqueStringCacheMappedMemory->unlock();
}

bool MUniqueStringCache::isLocked()
{
    return d_ptr->uniqueStringCacheMappedMemory->isLocked();
}

////////////////////////

MUniqueStringCacheLocker::MUniqueStringCacheLocker(MUniqueStringCache *usCache)
    : cache(usCache->lock() ? usCache : (MUniqueStringCache *)0)
{
    if (!cache) {
        mWarning("MUniqueStringCacheLocker") << "Unable to lock unique string cache" << usCache->d_ptr->uniqueStringCacheMappedMemory->accessSemaphore.errorString();
    }
}

MUniqueStringCacheLocker::~MUniqueStringCacheLocker()
{
    if (!cache) {
        return;
    }
    if (!cache->unlock()) {
        mWarning("MUniqueStringCacheLocker") << "Unable to unlock unique string cache" << cache->d_ptr->uniqueStringCacheMappedMemory->accessSemaphore.errorString();
    }
}

bool MUniqueStringCacheLocker::isLocked() const
{
    return cache;
}
<commit_msg>Fixes: NB#285637 - Notes application crashes while launching in memory full condition<commit_after>/***************************************************************************
**
** Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies).
** All rights reserved.
** Contact: Nokia Corporation (directui@nokia.com)
**
** This file is part of libmeegotouch.
**
** If you have questions regarding the use of this file, please contact
** Nokia at directui@nokia.com.
**
** This library is free software; you can redistribute it and/or
** modify it under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation
** and appearing in the file LICENSE.LGPL included in the packaging
** of this file.
**
****************************************************************************/

#include "muniquestringcache_p.h"
#include "muniquestringcache.h"

#include "mdebug.h"
#include "mcomponentdata.h"

#include <QFile>
#include <QFileInfo>
#include <QDir>
#include <QSystemSemaphore>
#include <QHash>
#include <QByteArray>
#include <QApplication>
#include <QScopedPointer>

namespace {
const int MAX_CACHE_SIZE = 1024*1024;
#define CACHE_NAME "MTF_UNIQUE_STRING_CACHE"

// slightly adapted from the version found in Qt's corelib/tools/qhash.cpp
static uint hash(const uchar *p)
{
    uint h = 0;

    while (*p) {
        h = (h << 4) + *p++;
        h ^= (h & 0xf0000000) >> 23;
        h &= 0x0fffffff;
    }
    return h;
}

}

uint qHash(const QLatin1String &key)
{
    return hash(reinterpret_cast<const uchar *>(key.latin1()));
}

class UniqueStringCacheMappedMemory
{
public:
    UniqueStringCacheMappedMemory(const QString& filename)
        : cacheFile(filename),
        accessSemaphore(filename + "_UNIQUE_STRING_SEMAPHORE", 1),
        rawMappedMemory(0),
        attached(false),
        locked(false)
    {
        if (!accessSemaphore.acquire()) {
            mWarning("UniqueStringCacheMappedMemory::UniqueStringCacheMappedMemory") << "Unable to aquire semaphore:" << accessSemaphore.errorString();
            return;
        }

        attachToCache();

        accessSemaphore.release();
    }

    bool isAttached() const { return attached; }

    uchar* data() { return rawMappedMemory; }
    QString errorString() { return accessSemaphore.errorString(); }

    ~UniqueStringCacheMappedMemory()
    {
    }

    bool isLocked()
    {
        return locked;
    }

    bool lock() {
        bool result = accessSemaphore.acquire();
        locked = result;
        return result;

    }
    bool unlock() {
        locked = false;
        return accessSemaphore.release();
    }

private:
    friend class MUniqueStringCacheLocker;

    void attachToCache()
    {
        if (!cacheFile.exists()) {
            if (!initializeCache()) {
                attached = false;
                return;
            }
        }

        if (!cacheFile.open(QIODevice::ReadWrite)) {
            mWarning("UniqueStringCacheMappedMemory") << "Could not open" <<
                cacheFile.fileName();
            return;
        }

        if (cacheFile.size() != MAX_CACHE_SIZE) {
            mWarning("UniqueStringCacheMappedMemory") << "Wrong cache file size" <<
                cacheFile.size() << "Expected:" << MAX_CACHE_SIZE;
            return;
        }

        rawMappedMemory = cacheFile.map(0, MAX_CACHE_SIZE);

        attached = true;

        cacheFile.close();
    }

    bool initializeCache() {
        if (!cacheFile.open(QFile::WriteOnly)) {
            //Maybe it failed because the directory doesn't exist
            QDir().mkpath(QFileInfo(cacheFile.fileName()).absolutePath());
            if (!cacheFile.open(QFile::WriteOnly)) {
                mWarning("UniqueStringCacheMappedMemory") <<
                         "Wrong permissions for cache directory. Cannot create" <<
                          cacheFile.fileName();
                return false;
            }
        }
        // resize the file and ensure that the file space gets allocated by writing a byte at the end of it
        cacheFile.resize(MAX_CACHE_SIZE);
        cacheFile.seek(MAX_CACHE_SIZE - 1);
        cacheFile.write("");
        cacheFile.close();
        return true;
    }

    QFile cacheFile;
    QSystemSemaphore accessSemaphore;
    uchar* rawMappedMemory;
    bool attached;
    bool locked;
};

MUniqueStringCachePrivate::MUniqueStringCachePrivate(MUniqueStringCache *parent, const QString &filename)
    : q_ptr(parent),
    filename(filename),
    uniqueStringCacheMappedMemory(new UniqueStringCacheMappedMemory(filename)),
    offset(sizeof(int)),
    cacheFilled(false),
    stringToIdCacheFilled(false)
{
}

MUniqueStringCachePrivate::~MUniqueStringCachePrivate()
{
    delete uniqueStringCacheMappedMemory;
}

// fills our datastructures from memory
// this method can be called several times to update the internal representation
// if the cache has been changed by another application
void MUniqueStringCachePrivate::fillUniqueStringCache() {
    uchar *rawCache = uniqueStringCacheMappedMemory->data();
    int elementsInCache = *reinterpret_cast<int*>(rawCache);
    int oldSize = idToPointerCache.count();
    idToPointerCache.resize(elementsInCache);

    for (int i = oldSize; i < elementsInCache; ++i) {
        uchar *stringAdress = rawCache + offset;
        int stringLength = *reinterpret_cast<int*>(stringAdress);

        idToPointerCache[i] = stringAdress;

        offset += sizeof(int) + stringLength + 1;

        if (stringToIdCacheFilled) {
            stringToIdCache.insert(QLatin1String(q_ptr->indexToString(i)), i);
        }
    }
    if (elementsInCache == 0) {
        // we want the empty string as first element
        insertStringToCache(QByteArray());
    }

    mDebug("MUniqueStringCachePrivate::fillUniqueStringCache")
            << QString("Elements in cache %1: %2, %3% filled").arg(filename).arg(elementsInCache).arg((float)offset/MAX_CACHE_SIZE*100);
}

// insert one string into the cache, makes sure to handle it gracefully when the cache has
// changed before it has been locked
int MUniqueStringCachePrivate::insertStringToCache(const QByteArray &string) {
    uchar *rawCache = uniqueStringCacheMappedMemory->data();
    int elementsInCache = *reinterpret_cast<int*>(rawCache);
    if (elementsInCache != idToPointerCache.count()) {
        // cache has changed, check if the string has been added
        // in the meanwhile
        fillUniqueStringCache();
        if (!stringToIdCacheFilled) {
            fillStringToIdCache();
        }
        StringToIdCache::const_iterator it = stringToIdCache.constFind(QLatin1String(string.constData()));
        if (it != stringToIdCache.constEnd()) {
            return *it;
        }
    }

    ++*reinterpret_cast<int*>(rawCache);

    uchar *stringAdress = rawCache + offset;
    *reinterpret_cast<int*>(stringAdress) = string.length();
    memcpy(stringAdress + sizeof(int), string.constData(), string.length() + 1);

    offset += sizeof(int) + string.length() + 1;
    if (offset >= MAX_CACHE_SIZE) {
        qFatal("unique string cache is full");
    }

    idToPointerCache.append((uchar*)stringAdress);
    stringToIdCache.insert(QLatin1String(reinterpret_cast<const char*>(stringAdress + sizeof(int))), idToPointerCache.count() - 1);

    return idToPointerCache.count() - 1;
}

// triggers initialization of our data structures from the cache if this
// did not happen yet
void MUniqueStringCachePrivate::initiallyFillCache() {
    if (!cacheFilled) {
        QScopedPointer<MUniqueStringCacheLocker> locker;
        if (!q_ptr->isLocked()) {
            locker.reset(new MUniqueStringCacheLocker(q_ptr));
            if (!locker->isLocked()) {
                return;
            }
        }
        fillUniqueStringCache();
        cacheFilled = true;
    }
}

void MUniqueStringCachePrivate::fillStringToIdCache()
{
    mDebug("MUniqueStringCachePrivate::fillStringToIdCache") << "filling stringToIdCache for" << filename;
    for (int i = 0; i < idToPointerCache.count(); ++i) {
        stringToIdCache.insert(QLatin1String(q_ptr->indexToString(i)), i);
    }
    stringToIdCacheFilled = true;
}

//////////////////////////////////////

MUniqueStringCache::MUniqueStringCache(const QString& filename)
    : d_ptr(new MUniqueStringCachePrivate(this, filename))
{
}

MUniqueStringCache::~MUniqueStringCache()
{
    delete d_ptr;
}

bool MUniqueStringCache::isAttached()
{
    return d_ptr->uniqueStringCacheMappedMemory->isAttached();
}

MUniqueStringCache::Index MUniqueStringCache::stringToIndex(const QByteArray& string)
{
    if (string.isEmpty()) {
        return EmptyStringIndex;
    }

    d_ptr->initiallyFillCache();

    if (!d_ptr->stringToIdCacheFilled) {
        d_ptr->fillStringToIdCache();
    }

    MUniqueStringCachePrivate::StringToIdCache::const_iterator it = d_ptr->stringToIdCache.constFind(QLatin1String(string.constData()));
    if (it != d_ptr->stringToIdCache.constEnd()) {
        return *it;
    }

    // string is unknown, we need to add it to the cache
    QScopedPointer<MUniqueStringCacheLocker> locker;
    if (!isLocked()) {
        locker.reset(new MUniqueStringCacheLocker(this));
        if (!locker->isLocked()) {
            return UndefinedIndex;
        }
    }
    return d_ptr->insertStringToCache(string);
}

QLatin1String MUniqueStringCache::indexToString(Index id)
{
    d_ptr->initiallyFillCache();
    if (id < 0) {
        return indexToString(EmptyStringIndex);
    } else if (id >= d_ptr->idToPointerCache.count())  {
        // it may be a new string in the cache. try to update the cache and then check again
        QScopedPointer<MUniqueStringCacheLocker> locker;
        if (!isLocked()) {
            locker.reset(new MUniqueStringCacheLocker(this));
            if (!locker->isLocked()) {
                return indexToString(EmptyStringIndex);
            }
        }
        d_ptr->fillUniqueStringCache();
        if (id >= d_ptr->idToPointerCache.count()) {
            mWarning("MUniqueStringCache::indexToString") << "Id" << id << "is unknown. Has the string cache been deleted?" << d_ptr->idToPointerCache.count();
            return indexToString(EmptyStringIndex);
        }
    }

    uchar *stringAdress = d_ptr->idToPointerCache.at(id);
    return QLatin1String(reinterpret_cast<const char*>(stringAdress + sizeof(int)));
}

bool MUniqueStringCache::lock()
{
    return d_ptr->uniqueStringCacheMappedMemory->lock();
}

bool MUniqueStringCache::unlock()
{
    return d_ptr->uniqueStringCacheMappedMemory->unlock();
}

bool MUniqueStringCache::isLocked()
{
    return d_ptr->uniqueStringCacheMappedMemory->isLocked();
}

////////////////////////

MUniqueStringCacheLocker::MUniqueStringCacheLocker(MUniqueStringCache *usCache)
    : cache(usCache->lock() ? usCache : (MUniqueStringCache *)0)
{
    if (!cache) {
        mWarning("MUniqueStringCacheLocker") << "Unable to lock unique string cache" << usCache->d_ptr->uniqueStringCacheMappedMemory->accessSemaphore.errorString();
    }
}

MUniqueStringCacheLocker::~MUniqueStringCacheLocker()
{
    if (!cache) {
        return;
    }
    if (!cache->unlock()) {
        mWarning("MUniqueStringCacheLocker") << "Unable to unlock unique string cache" << cache->d_ptr->uniqueStringCacheMappedMemory->accessSemaphore.errorString();
    }
}

bool MUniqueStringCacheLocker::isLocked() const
{
    return cache;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2017-2018 dresden elektronik ingenieurtechnik gmbh.
 * All rights reserved.
 *
 * The software in this package is published under the terms of the BSD
 * style license a copy of which has been included with this distribution in
 * the LICENSE.txt file.
 *
 */

#ifdef USE_WEBSOCKETS

#include "deconz/dbg_trace.h"
#include "deconz/util.h"
#include "websocket_server.h"

/*! Constructor.
 */
WebSocketServer::WebSocketServer(QObject *parent, quint16 port) :
    QObject(parent)
{
    srv = new QWebSocketServer("deconz", QWebSocketServer::NonSecureMode, this);

    quint16 p = 0;
    quint16 ports[] = { 443, 443, 8080, 8088, 20877, 0 }; // start with proxy frinedly ports first, use random port as fallback
    if (port > 0)
    {
        ports[0] = port;
    }

    QHostAddress address;
    if (deCONZ::appArgumentString("--http-listen", 0).isEmpty()) // Tie websocket server to specified IP, if any
    {
        address = QHostAddress::AnyIPv4;
    }
    else
    {
        address = deCONZ::appArgumentString("--http-listen", 0);
    }

    while (!srv->listen(address, ports[p]))
    {
        if (ports[p] == 0)
        {
            DBG_Printf(DBG_ERROR, "Giveup starting websocket server on %s, port: %u. error: %s\n", qPrintable(address.toString()), ports[p], qPrintable(srv->errorString()));
            break;
        }

        DBG_Printf(DBG_ERROR, "Failed to start websocket server on %s, port: %u. error: %s\n", qPrintable(address.toString()), ports[p], qPrintable(srv->errorString()));
        p++;
    }

    if (srv->isListening())
    {
        DBG_Printf(DBG_INFO, "Started websocket server on %s, port: %u\n", qPrintable(address.toString()), srv->serverPort());
        connect(srv, SIGNAL(newConnection()), this, SLOT(onNewConnection()));
    }
}

/*! Returns the websocket server port.
    \return the active server port, or 0 if not active
 */
quint16 WebSocketServer::port() const
{
    return srv->isListening() ? srv->serverPort() : 0;
}

/*! Handler for new client connections.
 */
void WebSocketServer::onNewConnection()
{
    while (srv->hasPendingConnections())
    {
        QWebSocket *sock = srv->nextPendingConnection();
        DBG_Printf(DBG_INFO, "New websocket %s:%u (state: %d) \n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), sock->state());
        connect(sock, SIGNAL(disconnected()), this, SLOT(onSocketDisconnected()));
        connect(sock, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(onSocketError(QAbstractSocket::SocketError)));
        clients.push_back(sock);
    }
}

/*! Handle websocket disconnected signal.
 */
void WebSocketServer::onSocketDisconnected()
{
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = qobject_cast<QWebSocket*>(sender());
        DBG_Assert(sock);
        if (sock && clients[i] == sock)
        {
            DBG_Printf(DBG_INFO, "Websocket disconnected %s:%u, state: %d, close-code: %d, reason: %s\n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), sock->state(), sock->closeCode(), qPrintable(sock->closeReason()));
            sock->deleteLater();
            clients[i] = clients.back();
            clients.pop_back();
        }
    }
}

/*! Handle websocket error signal.
    \param err - the error which occured
 */
void WebSocketServer::onSocketError(QAbstractSocket::SocketError err)
{
    Q_UNUSED(err);
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = qobject_cast<QWebSocket*>(sender());
        DBG_Assert(sock);
        if (sock && clients[i] == sock)
        {
            DBG_Printf(DBG_INFO, "Remove websocket %s:%u after error %s, close-code: %d, reason: %s\n",
                       qPrintable(sock->peerAddress().toString()), sock->peerPort(), qPrintable(sock->errorString()), sock->closeCode(), qPrintable(sock->closeReason()));
            sock->deleteLater();
            clients[i] = clients.back();
            clients.pop_back();
        }
    }
}

/*! Broadcasts a message to all connected clients.
    \param msg the message as JSON string
 */
void WebSocketServer::broadcastTextMessage(const QString &msg)
{
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = clients[i];

        if (sock->state() != QAbstractSocket::ConnectedState)
        {
            DBG_Printf(DBG_INFO, "Websocket %s:%u unexpected state: %d\n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), sock->state());
        }

        qint64 ret = sock->sendTextMessage(msg);
        DBG_Printf(DBG_INFO_L2, "Websocket %s:%u send message: %s (ret = %d)\n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), qPrintable(msg), ret);
        sock->flush();
    }
}

/*! Flush the sockets of all connected clients.
 */
void WebSocketServer::flush()
{
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = clients[i];

        if (sock->state() == QAbstractSocket::ConnectedState)
        {
            sock->flush();
        }
    }
}

#else // no websockets
  WebSocketServer::WebSocketServer(QObject *parent) :
      QObject(parent)
  { }
  void WebSocketServer::onNewConnection() { }
  void WebSocketServer::broadcastTextMessage(const QString &) { }
  quint16 WebSocketServer::port() const {  return 0; }
#endif
<commit_msg>Amend default value for deCONZ::appArgumentString<commit_after>/*
 * Copyright (c) 2017-2018 dresden elektronik ingenieurtechnik gmbh.
 * All rights reserved.
 *
 * The software in this package is published under the terms of the BSD
 * style license a copy of which has been included with this distribution in
 * the LICENSE.txt file.
 *
 */

#ifdef USE_WEBSOCKETS

#include "deconz/dbg_trace.h"
#include "deconz/util.h"
#include "websocket_server.h"

/*! Constructor.
 */
WebSocketServer::WebSocketServer(QObject *parent, quint16 port) :
    QObject(parent)
{
    srv = new QWebSocketServer("deconz", QWebSocketServer::NonSecureMode, this);

    quint16 p = 0;
    quint16 ports[] = { 443, 443, 8080, 8088, 20877, 0 }; // start with proxy frinedly ports first, use random port as fallback
    if (port > 0)
    {
        ports[0] = port;
    }

    QHostAddress address;
    if (deCONZ::appArgumentString("--http-listen", QString()).isEmpty()) // Tie websocket server to specified IP, if any
    {
        address = QHostAddress::AnyIPv4;
    }
    else
    {
        address = deCONZ::appArgumentString("--http-listen", QString());
    }

    while (!srv->listen(address, ports[p]))
    {
        if (ports[p] == 0)
        {
            DBG_Printf(DBG_ERROR, "Giveup starting websocket server on %s, port: %u. error: %s\n", qPrintable(address.toString()), ports[p], qPrintable(srv->errorString()));
            break;
        }

        DBG_Printf(DBG_ERROR, "Failed to start websocket server on %s, port: %u. error: %s\n", qPrintable(address.toString()), ports[p], qPrintable(srv->errorString()));
        p++;
    }

    if (srv->isListening())
    {
        DBG_Printf(DBG_INFO, "Started websocket server on %s, port: %u\n", qPrintable(address.toString()), srv->serverPort());
        connect(srv, SIGNAL(newConnection()), this, SLOT(onNewConnection()));
    }
}

/*! Returns the websocket server port.
    \return the active server port, or 0 if not active
 */
quint16 WebSocketServer::port() const
{
    return srv->isListening() ? srv->serverPort() : 0;
}

/*! Handler for new client connections.
 */
void WebSocketServer::onNewConnection()
{
    while (srv->hasPendingConnections())
    {
        QWebSocket *sock = srv->nextPendingConnection();
        DBG_Printf(DBG_INFO, "New websocket %s:%u (state: %d) \n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), sock->state());
        connect(sock, SIGNAL(disconnected()), this, SLOT(onSocketDisconnected()));
        connect(sock, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(onSocketError(QAbstractSocket::SocketError)));
        clients.push_back(sock);
    }
}

/*! Handle websocket disconnected signal.
 */
void WebSocketServer::onSocketDisconnected()
{
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = qobject_cast<QWebSocket*>(sender());
        DBG_Assert(sock);
        if (sock && clients[i] == sock)
        {
            DBG_Printf(DBG_INFO, "Websocket disconnected %s:%u, state: %d, close-code: %d, reason: %s\n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), sock->state(), sock->closeCode(), qPrintable(sock->closeReason()));
            sock->deleteLater();
            clients[i] = clients.back();
            clients.pop_back();
        }
    }
}

/*! Handle websocket error signal.
    \param err - the error which occured
 */
void WebSocketServer::onSocketError(QAbstractSocket::SocketError err)
{
    Q_UNUSED(err);
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = qobject_cast<QWebSocket*>(sender());
        DBG_Assert(sock);
        if (sock && clients[i] == sock)
        {
            DBG_Printf(DBG_INFO, "Remove websocket %s:%u after error %s, close-code: %d, reason: %s\n",
                       qPrintable(sock->peerAddress().toString()), sock->peerPort(), qPrintable(sock->errorString()), sock->closeCode(), qPrintable(sock->closeReason()));
            sock->deleteLater();
            clients[i] = clients.back();
            clients.pop_back();
        }
    }
}

/*! Broadcasts a message to all connected clients.
    \param msg the message as JSON string
 */
void WebSocketServer::broadcastTextMessage(const QString &msg)
{
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = clients[i];

        if (sock->state() != QAbstractSocket::ConnectedState)
        {
            DBG_Printf(DBG_INFO, "Websocket %s:%u unexpected state: %d\n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), sock->state());
        }

        qint64 ret = sock->sendTextMessage(msg);
        DBG_Printf(DBG_INFO_L2, "Websocket %s:%u send message: %s (ret = %d)\n", qPrintable(sock->peerAddress().toString()), sock->peerPort(), qPrintable(msg), ret);
        sock->flush();
    }
}

/*! Flush the sockets of all connected clients.
 */
void WebSocketServer::flush()
{
    for (size_t i = 0; i < clients.size(); i++)
    {
        QWebSocket *sock = clients[i];

        if (sock->state() == QAbstractSocket::ConnectedState)
        {
            sock->flush();
        }
    }
}

#else // no websockets
  WebSocketServer::WebSocketServer(QObject *parent) :
      QObject(parent)
  { }
  void WebSocketServer::onNewConnection() { }
  void WebSocketServer::broadcastTextMessage(const QString &) { }
  quint16 WebSocketServer::port() const {  return 0; }
#endif
<|endoftext|>


<commit_before>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude.SetValue( ( S.Latitude.Value()*Measurement.Latitude.Variance() + Measurement.Latitude.Value()*S.Latitude.Variance() ) / (S.Latitude.Variance()+Measurement.Latitude.Variance()) );
    result.Longitude.SetValue( ( S.Longitude.Value()*Measurement.Longitude.Variance() + Measurement.Longitude.Value()*S.Longitude.Variance() ) / (S.Longitude.Variance()+Measurement.Longitude.Variance()) );
    result.Heading.SetValue( ( S.Heading.Value()*Measurement.Heading.Variance() + Measurement.Heading.Value()*S.Heading.Variance() ) / (S.Heading.Variance()+Measurement.Heading.Variance()) );

    result.Latitude.SetVariance( 1.0 / (1.0/S.Latitude.Variance() + 1.0 / Measurement.Latitude.Variance()) );
    result.Longitude.SetVariance( 1.0 / (1.0/S.Longitude.Variance() + 1.0 / Measurement.Longitude.Variance()) );
    result.Heading.SetVariance( 1.0 / (1.0/S.Heading.Variance() + 1.0 / Measurement.Heading.Variance()) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude.SetValue(S.Latitude.Value() + Delta.Latitude.Value());
    result.Longitude.SetValue(S.Longitude.Value() + Delta.Longitude.Value());
    result.Heading.SetValue(S.Heading.Value() + Delta.Heading.Value());

    result.Latitude.SetVariance(S.Latitude.Variance() + Delta.Latitude.Variance());
    result.Longitude.SetVariance(S.Longitude.Variance() + Delta.Longitude.Variance());
    result.Heading.SetVariance(S.Heading.Variance() + Delta.Heading.Variance());
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::Instance().getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading.Value() * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading.Value() + W*t) * M_PI/180.0;
    delta.Heading.SetValue(W*t);
    delta.Latitude.SetValue(R*(cos(theta) - cos(thetaPrime)));
    delta.Longitude.SetValue(R*(sin(thetaPrime) - sin(theta)));
    // TODO - handle variances
    return delta;
}

Position PositionTracker::MeasurementFromIMUData(IMUData data)
{
    // Uses the "Destination point given distance and bearing from start point" described here:
    // http://www.movable-type.co.uk/scripts/latlong.html#destPoint
    Position measurement;
    double lat1 = _current_estimate.Latitude.Value();
    double lon1 = _current_estimate.Latitude.Value();
    double hed1 = _current_estimate.Heading.Value();
    double dx = data.X /* * time*time */;
    double dy = data.Y /* * time*time */;
    double d = sqrt(dx*dx + dy*dy); // Distance travelled
    double R = 6378137; // radius of Earth
    measurement.Latitude.SetValue(asin(sin(lat1)*cos(d/R)+cos(lat1)*sin(d/R)*cos(hed1)));
    measurement.Longitude.SetValue(lon1 + atan2(sin(hed1)*sin(d/R)*cos(lat1),cos(d/R)-sin(lat1)*sin(measurement.Latitude.Value())));
    measurement.Heading.SetValue(atan2(measurement.Longitude.Value()-lon1)*sin(lat1), cos(measurement.Latitude.Value())*sin(lat1)-sin(measurement.Latitude.Value())*cos(lat1)*cos(measurement.Longitude.Value()-lon1));
    measurement.Heading.SetValue((measurement.Heading.Value()+180) % 360);
    // TODO - handle variances
    return measurement;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude.SetValue(data.Lat());
    measurement.Longitude.SetValue(data.Long());
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading.SetValue(data.Heading());
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{
    _current_estimate = UpdateWithMeasurement(_current_estimate, MeasurementFromIMUData(data));
}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}
<commit_msg>Fixes syntax errors in MeasurementFromIMUData(...)<commit_after>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude.SetValue( ( S.Latitude.Value()*Measurement.Latitude.Variance() + Measurement.Latitude.Value()*S.Latitude.Variance() ) / (S.Latitude.Variance()+Measurement.Latitude.Variance()) );
    result.Longitude.SetValue( ( S.Longitude.Value()*Measurement.Longitude.Variance() + Measurement.Longitude.Value()*S.Longitude.Variance() ) / (S.Longitude.Variance()+Measurement.Longitude.Variance()) );
    result.Heading.SetValue( ( S.Heading.Value()*Measurement.Heading.Variance() + Measurement.Heading.Value()*S.Heading.Variance() ) / (S.Heading.Variance()+Measurement.Heading.Variance()) );

    result.Latitude.SetVariance( 1.0 / (1.0/S.Latitude.Variance() + 1.0 / Measurement.Latitude.Variance()) );
    result.Longitude.SetVariance( 1.0 / (1.0/S.Longitude.Variance() + 1.0 / Measurement.Longitude.Variance()) );
    result.Heading.SetVariance( 1.0 / (1.0/S.Heading.Variance() + 1.0 / Measurement.Heading.Variance()) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude.SetValue(S.Latitude.Value() + Delta.Latitude.Value());
    result.Longitude.SetValue(S.Longitude.Value() + Delta.Longitude.Value());
    result.Heading.SetValue(S.Heading.Value() + Delta.Heading.Value());

    result.Latitude.SetVariance(S.Latitude.Variance() + Delta.Latitude.Variance());
    result.Longitude.SetVariance(S.Longitude.Variance() + Delta.Longitude.Variance());
    result.Heading.SetVariance(S.Heading.Variance() + Delta.Heading.Variance());
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::Instance().getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading.Value() * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading.Value() + W*t) * M_PI/180.0;
    delta.Heading.SetValue(W*t);
    delta.Latitude.SetValue(R*(cos(theta) - cos(thetaPrime)));
    delta.Longitude.SetValue(R*(sin(thetaPrime) - sin(theta)));
    // TODO - handle variances
    return delta;
}

Position PositionTracker::MeasurementFromIMUData(IMUData data)
{
    // Uses the "Destination point given distance and bearing from start point" described here:
    // http://www.movable-type.co.uk/scripts/latlong.html#destPoint
    Position measurement;
    double lat1 = _current_estimate.Latitude.Value();
    double lon1 = _current_estimate.Latitude.Value();
    double hed1 = _current_estimate.Heading.Value();
    double dx = data.X /* * time*time */;
    double dy = data.Y /* * time*time */;
    double d = sqrt(dx*dx + dy*dy); // Distance travelled
    double R = 6378137; // radius of Earth
    measurement.Latitude.SetValue(asin(sin(lat1)*cos(d/R)+cos(lat1)*sin(d/R)*cos(hed1)));
    measurement.Longitude.SetValue(lon1 + atan2(sin(hed1)*sin(d/R)*cos(lat1),cos(d/R)-sin(lat1)*sin(measurement.Latitude.Value())));
    measurement.Heading.SetValue(atan2((measurement.Longitude.Value()-lon1)*sin(lat1), cos(measurement.Latitude.Value())*sin(lat1)-sin(measurement.Latitude.Value())*cos(lat1)*cos(measurement.Longitude.Value()-lon1)));
    measurement.Heading.SetValue(measurement.Heading.Value()+180);
    while(measurement.Heading.Value() >= 360)
        measurement.Heading.SetValue(measurement.Heading.Value() - 360);
    while(measurement.Heading.Value() < 0)
        measurement.Heading.SetValue(measurement.Heading.Value() + 360);
    // TODO - handle variances
    return measurement;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude.SetValue(data.Lat());
    measurement.Longitude.SetValue(data.Long());
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading.SetValue(data.Heading());
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{
    _current_estimate = UpdateWithMeasurement(_current_estimate, MeasurementFromIMUData(data));
}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}
<|endoftext|>


<commit_before>#pragma once

#include <cstdint>
#include <string>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "blackhole/utils/format.hpp"

namespace blackhole {

namespace sink {

struct timer_t {
    std::time_t current() const {
        return std::time(nullptr);
    }
};

//! Tag for file sinks with no rotation.
template<class Backend, class Timer = timer_t> class NoRotation;

namespace rotator {

struct config_t {
    std::string pattern;
    std::uint16_t backups;
    std::uint64_t size;

    config_t(const std::string& pattern = ".%N", std::uint16_t backups = 5, std::uint64_t size = 10 * 1024 * 1024) :
        pattern(pattern),
        backups(backups),
        size(size)
    {}
};

} // namespace rotator

namespace time {

template<typename Backend>
struct ascending {
    Backend& backend;

    ascending(Backend& backend) : backend(backend) {}

    bool operator ()(const std::string& lhs, const std::string& rhs) const {
        return backend.changed(lhs) < backend.changed(rhs);
    }
};

}

namespace matcher {

struct datetime_t {
    const std::string& pattern;
    const std::uint16_t backups;

    datetime_t(const std::string& pattern, std::uint16_t backups) :
        pattern(pattern),
        backups(backups)
    {}

    bool operator ()(const std::string& filename) const {
        return !match(filename);
    }

    bool match(const std::string& filename) const {
        auto f_it = filename.begin();
        auto f_end = filename.end();
        auto p_it = pattern.begin();
        auto p_end = pattern.end();

        bool placeholder_expected = false;
        while (f_it != filename.end() && p_it != pattern.end()) {
            char f_c = *f_it;
            char p_c = *p_it;

            if (!placeholder_expected) {
                if (p_c == '%') {
                    placeholder_expected = true;
                    p_it++;
                } else if (f_c == p_c) {
                    f_it++;
                    p_it++;
                } else {
                    return false;
                }
            } else {
                switch (p_c) {
                case '%':
                    if (p_c == f_c) {
                        ++p_it;
                        ++f_it;
                        break;
                    } else {
                        return false;
                    }
                case 'M':
                case 'H':
                case 'd':
                case 'm':
                    if (!scan_digits(f_it, f_end, 2)) {
                        return false;
                    }
                    ++p_it;
                    break;
                case 'Y':
                    if (!scan_digits(f_it, f_end, 4)) {
                        return false;
                    }
                    ++p_it;
                    break;
                case 'N':
                    if (!scan_digits(f_it, f_end, digits(backups))) {
                        return false;
                    }
                    ++p_it;
                    break;
                default:
                    throw std::invalid_argument("Unsupported placeholder used in pattern for file scanning");
                }

                placeholder_expected = false;
            }
        }

        if (p_it == p_end) {
            if (f_it != f_end) {
                return scan_digits(f_it, f_end, std::distance(f_it, f_end));
            } else {
                return true;
            }
        } else {
            return false;
        }
    }

private:
    static bool scan_digits(std::string::const_iterator& it, std::string::const_iterator end, int n) {
        for (; n > 0; --n) {
            if (it == end) {
                return false;
            }

            const char c = *it++;
            if (!std::isdigit(c)) {
                return false;
            }
        }

        return true;
    }

    static uint digits(uint number) {
        uint digits = 0;
        while (number) {
            number /= 10;
            digits++;
        }

        return digits;
    }
};

} // namespace match

template<class Backend, class Timer = timer_t>
class rotator_t {
    rotator::config_t config;
    Backend& backend;
    Timer m_timer;
public:
    static const char* name() {
        return "rotate";
    }

    rotator_t(Backend& backend) :
        backend(backend)
    {}

    rotator_t(const rotator::config_t& config, Backend& backend) :
        config(config),
        backend(backend)
    {}

    Timer& timer() {
        return m_timer;
    }

    bool necessary() const {
        // In case of size based rotation everything is simple: file.size() + message.size() >= size;
        // In case of datetime based rotation that's seems to be a bit difficult: get_current_time() >= current_fence().
        return false;
    }

    void rotate() const {
        backend.flush();
        backend.close();

        const std::string& filename = backend.filename();

        std::string pattern = config.pattern;
        if (config.pattern.find("%(filename)s") != std::string::npos) {
            boost::algorithm::replace_all(pattern, "%(filename)s", filename);
        }

        rotate(backend.listdir(), pattern);

        if (backend.exists(filename)) {
            backend.rename(filename, format(pattern));
        }

        backend.open();
    }

private:
    void rotate(std::vector<std::string> filenames, const std::string& pattern) const {
        filter(&filenames, matcher::datetime_t(pattern, config.backups));
        std::sort(filenames.begin(), filenames.end(), time::ascending<Backend>(backend));

        std::vector<std::pair<std::string, std::string>> pairs = cumilative(filenames, pattern, config.backups);

        for (auto it = pairs.begin(); it != pairs.end(); ++it) {
            const std::pair<std::string, std::string>& pair = *it;
            if (backend.exists(pair.first)) {
                backend.rename(pair.first, pair.second);
            }
        }
    }

    template<typename Filter>
    void filter(std::vector<std::string>* filenames, Filter filter) const {
        filenames->erase(std::remove_if(filenames->begin(), filenames->end(), filter), filenames->end());
    }

    std::vector<std::pair<std::string, std::string> >
    cumilative(const std::vector<std::string>& filenames, const std::string& pattern, int backups) const {
        std::vector<std::pair<std::string, std::string>> result;
        int pos = pattern.find("%N");
        if (pos == -1) {
            return result;
        }

        bool placeholder_expected = false;
        for (auto it = pattern.begin(); it != pattern.end() && it != pattern.begin() + pos; ++it) {
            char c = *it;

            if (!placeholder_expected) {
                if (c == '%') {
                    placeholder_expected = true;
                }
            } else {
                switch (c) {
                case 'M':
                case 'H':
                case 'd':
                case 'm':
                case 'N':
                    break;
                case 'Y':
                    pos += 2;
                    break;
                default:
                    throw std::invalid_argument(utils::format("Unsupported placeholder used in pattern for file scanning: %s", c));
                }

                placeholder_expected = false;
            }
        }

        int counter = 0;
        for (auto it = filenames.begin(); it != filenames.end(); ++it) {
            const std::string& filename = *it;
            if (counter >= backups) {
                break;
            }

            std::string fn = filename;
            std::string current = std::string(fn.begin() + pos, fn.begin() + pos + 1);
            int i = atoi(current.data());
            i++;
            fn.replace(pos, 1, boost::lexical_cast<std::string>(i));
            result.push_back(std::make_pair(filename, fn));
            counter++;
        }
        return result;
    }

    std::string format(const std::string& pattern) const {
        std::string filename = pattern;
        boost::algorithm::replace_all(filename, "%N", "1");
        std::time_t time = m_timer.current();
        char buf[128];
        strftime(buf, 128, filename.data(), std::gmtime(&time));
        return buf;
    }
};

} // namespace sink

} // namespace blackhole
<commit_msg>Code style.<commit_after>#pragma once

#include <cstdint>
#include <string>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "blackhole/utils/format.hpp"

namespace blackhole {

namespace sink {

struct timer_t {
    std::time_t current() const {
        return std::time(nullptr);
    }
};

//! Tag for file sinks with no rotation.
template<class Backend, class Timer = timer_t> class NoRotation;

namespace rotator {

struct config_t {
    std::string pattern;
    std::uint16_t backups;
    std::uint64_t size;

    config_t(const std::string& pattern = ".%N", std::uint16_t backups = 5, std::uint64_t size = 10 * 1024 * 1024) :
        pattern(pattern),
        backups(backups),
        size(size)
    {}
};

} // namespace rotator

namespace time {

template<typename Backend>
struct ascending {
    Backend& backend;

    ascending(Backend& backend) : backend(backend) {}

    bool operator ()(const std::string& lhs, const std::string& rhs) const {
        return backend.changed(lhs) < backend.changed(rhs);
    }
};

} // namespace time

namespace matcher {

struct datetime_t {
    const std::string& pattern;
    const std::uint16_t backups;

    datetime_t(const std::string& pattern, std::uint16_t backups) :
        pattern(pattern),
        backups(backups)
    {}

    bool operator ()(const std::string& filename) const {
        return !match(filename);
    }

    bool match(const std::string& filename) const {
        auto f_it = filename.begin();
        auto f_end = filename.end();
        auto p_it = pattern.begin();
        auto p_end = pattern.end();

        bool placeholder_expected = false;
        while (f_it != filename.end() && p_it != pattern.end()) {
            char f_c = *f_it;
            char p_c = *p_it;

            if (!placeholder_expected) {
                if (p_c == '%') {
                    placeholder_expected = true;
                    p_it++;
                } else if (f_c == p_c) {
                    f_it++;
                    p_it++;
                } else {
                    return false;
                }
            } else {
                switch (p_c) {
                case '%':
                    if (p_c == f_c) {
                        ++p_it;
                        ++f_it;
                        break;
                    } else {
                        return false;
                    }
                case 'M':
                case 'H':
                case 'd':
                case 'm':
                    if (!scan_digits(f_it, f_end, 2)) {
                        return false;
                    }
                    ++p_it;
                    break;
                case 'Y':
                    if (!scan_digits(f_it, f_end, 4)) {
                        return false;
                    }
                    ++p_it;
                    break;
                case 'N':
                    if (!scan_digits(f_it, f_end, digits(backups))) {
                        return false;
                    }
                    ++p_it;
                    break;
                default:
                    throw std::invalid_argument("Unsupported placeholder used in pattern for file scanning");
                }

                placeholder_expected = false;
            }
        }

        if (p_it == p_end) {
            if (f_it != f_end) {
                return scan_digits(f_it, f_end, std::distance(f_it, f_end));
            } else {
                return true;
            }
        } else {
            return false;
        }
    }

private:
    static bool scan_digits(std::string::const_iterator& it, std::string::const_iterator end, int n) {
        for (; n > 0; --n) {
            if (it == end) {
                return false;
            }

            const char c = *it++;
            if (!std::isdigit(c)) {
                return false;
            }
        }

        return true;
    }

    static uint digits(uint number) {
        uint digits = 0;
        while (number) {
            number /= 10;
            digits++;
        }

        return digits;
    }
};

} // namespace match

template<class Backend, class Timer = timer_t>
class rotator_t {
    rotator::config_t config;
    Backend& backend;
    Timer m_timer;
public:
    static const char* name() {
        return "rotate";
    }

    rotator_t(Backend& backend) :
        backend(backend)
    {}

    rotator_t(const rotator::config_t& config, Backend& backend) :
        config(config),
        backend(backend)
    {}

    Timer& timer() {
        return m_timer;
    }

    bool necessary() const {
        // In case of size based rotation everything is simple: file.size() + message.size() >= size;
        // In case of datetime based rotation that's seems to be a bit difficult: get_current_time() >= current_fence().
        return false;
    }

    void rotate() const {
        backend.flush();
        backend.close();

        const std::string& filename = backend.filename();

        std::string pattern = config.pattern;
        if (config.pattern.find("%(filename)s") != std::string::npos) {
            boost::algorithm::replace_all(pattern, "%(filename)s", filename);
        }

        rotate(backend.listdir(), pattern);

        if (backend.exists(filename)) {
            backend.rename(filename, format(pattern));
        }

        backend.open();
    }

private:
    void rotate(std::vector<std::string> filenames, const std::string& pattern) const {
        filter(&filenames, matcher::datetime_t(pattern, config.backups));
        std::sort(filenames.begin(), filenames.end(), time::ascending<Backend>(backend));

        std::vector<std::pair<std::string, std::string>> pairs = cumilative(filenames, pattern, config.backups);

        for (auto it = pairs.begin(); it != pairs.end(); ++it) {
            const std::pair<std::string, std::string>& pair = *it;
            if (backend.exists(pair.first)) {
                backend.rename(pair.first, pair.second);
            }
        }
    }

    template<typename Filter>
    void filter(std::vector<std::string>* filenames, Filter filter) const {
        filenames->erase(std::remove_if(filenames->begin(), filenames->end(), filter), filenames->end());
    }

    std::vector<std::pair<std::string, std::string> >
    cumilative(const std::vector<std::string>& filenames, const std::string& pattern, int backups) const {
        std::vector<std::pair<std::string, std::string>> result;
        int pos = pattern.find("%N");
        if (pos == -1) {
            return result;
        }

        bool placeholder_expected = false;
        for (auto it = pattern.begin(); it != pattern.end() && it != pattern.begin() + pos; ++it) {
            char c = *it;

            if (!placeholder_expected) {
                if (c == '%') {
                    placeholder_expected = true;
                }
            } else {
                switch (c) {
                case 'M':
                case 'H':
                case 'd':
                case 'm':
                case 'N':
                    break;
                case 'Y':
                    pos += 2;
                    break;
                default:
                    throw std::invalid_argument(utils::format("Unsupported placeholder used in pattern for file scanning: %s", c));
                }

                placeholder_expected = false;
            }
        }

        int counter = 0;
        for (auto it = filenames.begin(); it != filenames.end(); ++it) {
            const std::string& filename = *it;
            if (counter >= backups) {
                break;
            }

            std::string fn = filename;
            std::string current = std::string(fn.begin() + pos, fn.begin() + pos + 1);
            int i = atoi(current.data());
            i++;
            fn.replace(pos, 1, boost::lexical_cast<std::string>(i));
            result.push_back(std::make_pair(filename, fn));
            counter++;
        }
        return result;
    }

    std::string format(const std::string& pattern) const {
        std::string filename = pattern;
        boost::algorithm::replace_all(filename, "%N", "1");
        std::time_t time = m_timer.current();
        char buf[128];
        strftime(buf, 128, filename.data(), std::gmtime(&time));
        return buf;
    }
};

} // namespace sink

} // namespace blackhole
<|endoftext|>


<commit_before>#include "batoid.h"
#include <pybind11/pybind11.h>

namespace py = pybind11;

namespace batoid {
    void pyExportVec3(py::module&);
    void pyExportVec2(py::module&);
    void pyExportRay(py::module&);
    void pyExportIntersection(py::module&);
    void pyExportSurface(py::module&);
    void pyExportParaboloid(py::module&);
    void pyExportAsphere(py::module&);
    void pyExportQuadric(py::module&);
    void pyExportSphere(py::module&);
    void pyExportPlane(py::module&);
    void pyExportTransformation(py::module&);
    void pyExportTable(py::module&);
    void pyExportMedium(py::module&);
    void pyExportObscuration(py::module&);

    PYBIND11_PLUGIN(_batoid) {
        py::module m("_batoid", "ray tracer");
        pyExportVec3(m);
        pyExportVec2(m);
        pyExportRay(m);
        pyExportIntersection(m);
        pyExportSurface(m);
        pyExportParaboloid(m);
        pyExportAsphere(m);
        pyExportQuadric(m);
        pyExportSphere(m);
        pyExportPlane(m);
        pyExportTransformation(m);
        pyExportTable(m);
        pyExportMedium(m);
        pyExportObscuration(m);

        return m.ptr();
    }
}
<commit_msg>Fix PLUGIN/MODULE deprecation<commit_after>#include "batoid.h"
#include <pybind11/pybind11.h>

namespace py = pybind11;

namespace batoid {
    void pyExportVec3(py::module&);
    void pyExportVec2(py::module&);
    void pyExportRay(py::module&);
    void pyExportIntersection(py::module&);
    void pyExportSurface(py::module&);
    void pyExportParaboloid(py::module&);
    void pyExportAsphere(py::module&);
    void pyExportQuadric(py::module&);
    void pyExportSphere(py::module&);
    void pyExportPlane(py::module&);
    void pyExportTransformation(py::module&);
    void pyExportTable(py::module&);
    void pyExportMedium(py::module&);
    void pyExportObscuration(py::module&);

#if (PYBIND11_VERSION_MAJOR >= 2) & (PYBIND11_VERSION_MINOR >= 2)
    PYBIND11_MODULE(_batoid, m) {
#else
    PYBIND11_PLUGIN(_batoid) {
        py::module m("_batoid", "ray tracer");
#endif
        pyExportVec3(m);
        pyExportVec2(m);
        pyExportRay(m);
        pyExportIntersection(m);
        pyExportSurface(m);
        pyExportParaboloid(m);
        pyExportAsphere(m);
        pyExportQuadric(m);
        pyExportSphere(m);
        pyExportPlane(m);
        pyExportTransformation(m);
        pyExportTable(m);
        pyExportMedium(m);
        pyExportObscuration(m);
#if !((PYBIND11_VERSION_MAJOR >= 2) & (PYBIND11_VERSION_MINOR >= 2))
        return m.ptr();
#endif
    }
}
<|endoftext|>