<commit_before>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2000, 2010 Oracle and/or its affiliates.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_sc.hxx"

#undef SC_DLLIMPLEMENTATION

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

#include <svx/dlgutil.hxx>
#include <svx/drawitem.hxx>
#include <svx/xtable.hxx>

#include "appoptio.hxx"
#include "scmod.hxx"
#include "scitems.hxx"
#include "tpview.hxx"
#include "global.hxx"
#include "viewopti.hxx"
#include "tabvwsh.hxx"
#include "uiitems.hxx"
#include "scresid.hxx"
#include "docsh.hxx"
#include "sc.hrc"       // -> Slot-IDs
#include "optdlg.hrc"
#include "globstr.hrc"

#include "opredlin.hxx"
#include "opredlin.hrc"

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

ScRedlineOptionsTabPage::ScRedlineOptionsTabPage( Window* pParent,
                                                    const SfxItemSet& rSet )
    : SfxTabPage(pParent, ScResId(RID_SCPAGE_OPREDLINE), rSet),
    aContentFT      ( this, ScResId(FT_CONTENT  )),
    aContentColorLB ( this, ScResId(CLB_CONTENT )),
    aRemoveFT       ( this, ScResId(FT_REMOVE   )),
    aRemoveColorLB  ( this, ScResId(CLB_REMOVE  )),
    aInsertFT       ( this, ScResId(FT_INSERT   )),
    aInsertColorLB  ( this, ScResId(CLB_INSERT  )),
    aMoveFT         ( this, ScResId(FT_MOVE     )),
    aMoveColorLB    ( this, ScResId(CLB_MOVE    )),
    aChangedGB      ( this, ScResId(GB_COLORCHGS)),
    aAuthorStr      (ScResId(STR_AUTHOR))
{
    FreeResource();

    Link aLk = LINK(this, ScRedlineOptionsTabPage, ColorHdl);
    aContentColorLB.SetSelectHdl( aLk );
    aMoveColorLB.SetSelectHdl( aLk );
    aInsertColorLB.SetSelectHdl( aLk );
    aRemoveColorLB.SetSelectHdl( aLk );
}

/*-----------------------------------------------------------------------
    Beschreibung:
 -----------------------------------------------------------------------*/

__EXPORT ScRedlineOptionsTabPage::~ScRedlineOptionsTabPage()
{
}

/*-----------------------------------------------------------------------
    Beschreibung:
 -----------------------------------------------------------------------*/

SfxTabPage* __EXPORT ScRedlineOptionsTabPage::Create( Window* pParent, const SfxItemSet& rSet )
{
    return new ScRedlineOptionsTabPage( pParent, rSet );
}

/*-----------------------------------------------------------------------
    Beschreibung:
 -----------------------------------------------------------------------*/

BOOL __EXPORT ScRedlineOptionsTabPage::FillItemSet( SfxItemSet& /* rSet */ )
{
    ScAppOptions aAppOptions=SC_MOD()->GetAppOptions();

    ULONG nNew=0;
    USHORT nPos=0;

    nPos = aContentColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aContentColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aContentColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackContentColor(nNew);

    }
    nPos = aMoveColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aMoveColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aMoveColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackMoveColor(nNew);

    }
    nPos = aInsertColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aInsertColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aInsertColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackInsertColor(nNew);

    }
    nPos = aRemoveColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aRemoveColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aRemoveColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackDeleteColor(nNew);

    }

    SC_MOD()->SetAppOptions(aAppOptions);

    //  Repaint (wenn alles ueber Items laufen wuerde, wie es sich gehoert,
    //  waere das nicht noetig...)
    ScDocShell* pDocSh = PTR_CAST(ScDocShell, SfxObjectShell::Current());
    if (pDocSh)
        pDocSh->PostPaintGridAll();

    return FALSE;
}

/*-----------------------------------------------------------------------
    Beschreibung:
 -----------------------------------------------------------------------*/

void __EXPORT ScRedlineOptionsTabPage::Reset( const SfxItemSet& /* rSet */ )
{

    XColorTable* pColorTbl = XColorTable::GetStdColorTable();
    aContentColorLB.InsertEntry(aAuthorStr);
    aMoveColorLB.InsertEntry(aAuthorStr);
    aInsertColorLB.InsertEntry(aAuthorStr);
    aRemoveColorLB.InsertEntry(aAuthorStr);

    aContentColorLB.SetUpdateMode( FALSE);
    aMoveColorLB.SetUpdateMode( FALSE);
    aInsertColorLB.SetUpdateMode( FALSE);
    aRemoveColorLB.SetUpdateMode( FALSE);

    for( USHORT i = 0; i < pColorTbl->Count(); ++i )
    {
        XColorEntry* pEntry = pColorTbl->GetColor( i );
        Color aColor = pEntry->GetColor();
        String sName = pEntry->GetName();

        aContentColorLB.InsertEntry( aColor, sName );
        aMoveColorLB.InsertEntry( aColor, sName );
        aInsertColorLB.InsertEntry( aColor, sName );
        aRemoveColorLB.InsertEntry( aColor, sName );
    }
    aContentColorLB.SetUpdateMode( TRUE );
    aMoveColorLB.SetUpdateMode( TRUE );
    aInsertColorLB.SetUpdateMode( TRUE );
    aRemoveColorLB.SetUpdateMode( TRUE );

    ScAppOptions aAppOptions=SC_MOD()->GetAppOptions();

    ULONG nColor = aAppOptions.GetTrackContentColor();
    if (nColor == COL_TRANSPARENT)
        aContentColorLB.SelectEntryPos(0);
    else
        aContentColorLB.SelectEntry(Color(nColor));

    nColor = aAppOptions.GetTrackMoveColor();
    if (nColor == COL_TRANSPARENT)
        aMoveColorLB.SelectEntryPos(0);
    else
        aMoveColorLB.SelectEntry(Color(nColor));

    nColor = aAppOptions.GetTrackInsertColor();
    if (nColor == COL_TRANSPARENT)
        aInsertColorLB.SelectEntryPos(0);
    else
        aInsertColorLB.SelectEntry(Color(nColor));

    nColor = aAppOptions.GetTrackDeleteColor();
    if (nColor == COL_TRANSPARENT)
        aRemoveColorLB.SelectEntryPos(0);
    else
        aRemoveColorLB.SelectEntry(Color(nColor));

}

IMPL_LINK( ScRedlineOptionsTabPage, ColorHdl, ColorListBox *, EMPTYARG )
{
/*
    SvxFontPrevWindow *pPrev;
    ListBox *pLB;

    if (pColorLB == &aInsertColorLB)
    {
        pPrev = &aInsertPreviewWN;
        pLB = &aInsertLB;
    }
    else
    {
        pPrev = &aDeletedPreviewWN;
        pLB = &aDeletedLB;
    }

    SvxFont& rFont = pPrev->GetFont();
    USHORT nPos = pLB->GetSelectEntryPos();
    if (nPos == LISTBOX_ENTRY_NOTFOUND)
        nPos = 0;

    CharAttr *pAttr = (CharAttr *)pLB->GetEntryData(nPos);

    if (pAttr->nItemId == SID_ATTR_BRUSH)
    {
        rFont.SetColor(Color(COL_BLACK));
        nPos = pColorLB->GetSelectEntryPos();
        if (nPos && nPos != LISTBOX_ENTRY_NOTFOUND)
        {
            Brush aBrush(Color(pColorLB->GetSelectEntryColor()));
            pPrev->SetBrush(aBrush);
        }
        else
        {
            Brush aBrush(Color(COL_LIGHTGRAY));
            pPrev->SetBrush(aBrush);
        }
    }
    else
    {
        nPos = pColorLB->GetSelectEntryPos();
        if (nPos && nPos != LISTBOX_ENTRY_NOTFOUND)
            rFont.SetColor(pColorLB->GetEntryColor(nPos));
        else
            rFont.SetColor(Color(COL_RED));
    }

    pPrev->Invalidate();
*/
    return 0;
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<commit_msg>Remove empty Beschreibung (Description) comments<commit_after>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2000, 2010 Oracle and/or its affiliates.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_sc.hxx"

#undef SC_DLLIMPLEMENTATION

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

#include <svx/dlgutil.hxx>
#include <svx/drawitem.hxx>
#include <svx/xtable.hxx>

#include "appoptio.hxx"
#include "scmod.hxx"
#include "scitems.hxx"
#include "tpview.hxx"
#include "global.hxx"
#include "viewopti.hxx"
#include "tabvwsh.hxx"
#include "uiitems.hxx"
#include "scresid.hxx"
#include "docsh.hxx"
#include "sc.hrc"       // -> Slot-IDs
#include "optdlg.hrc"
#include "globstr.hrc"

#include "opredlin.hxx"
#include "opredlin.hrc"

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

ScRedlineOptionsTabPage::ScRedlineOptionsTabPage( Window* pParent,
                                                    const SfxItemSet& rSet )
    : SfxTabPage(pParent, ScResId(RID_SCPAGE_OPREDLINE), rSet),
    aContentFT      ( this, ScResId(FT_CONTENT  )),
    aContentColorLB ( this, ScResId(CLB_CONTENT )),
    aRemoveFT       ( this, ScResId(FT_REMOVE   )),
    aRemoveColorLB  ( this, ScResId(CLB_REMOVE  )),
    aInsertFT       ( this, ScResId(FT_INSERT   )),
    aInsertColorLB  ( this, ScResId(CLB_INSERT  )),
    aMoveFT         ( this, ScResId(FT_MOVE     )),
    aMoveColorLB    ( this, ScResId(CLB_MOVE    )),
    aChangedGB      ( this, ScResId(GB_COLORCHGS)),
    aAuthorStr      (ScResId(STR_AUTHOR))
{
    FreeResource();

    Link aLk = LINK(this, ScRedlineOptionsTabPage, ColorHdl);
    aContentColorLB.SetSelectHdl( aLk );
    aMoveColorLB.SetSelectHdl( aLk );
    aInsertColorLB.SetSelectHdl( aLk );
    aRemoveColorLB.SetSelectHdl( aLk );
}

__EXPORT ScRedlineOptionsTabPage::~ScRedlineOptionsTabPage()
{
}

SfxTabPage* __EXPORT ScRedlineOptionsTabPage::Create( Window* pParent, const SfxItemSet& rSet )
{
    return new ScRedlineOptionsTabPage( pParent, rSet );
}

BOOL __EXPORT ScRedlineOptionsTabPage::FillItemSet( SfxItemSet& /* rSet */ )
{
    ScAppOptions aAppOptions=SC_MOD()->GetAppOptions();

    ULONG nNew=0;
    USHORT nPos=0;

    nPos = aContentColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aContentColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aContentColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackContentColor(nNew);

    }
    nPos = aMoveColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aMoveColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aMoveColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackMoveColor(nNew);

    }
    nPos = aInsertColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aInsertColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aInsertColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackInsertColor(nNew);

    }
    nPos = aRemoveColorLB.GetSelectEntryPos();
    if (nPos != LISTBOX_ENTRY_NOTFOUND)
    {
        nPos = aRemoveColorLB.GetSelectEntryPos();
        if (nPos!=0)
            nNew= aRemoveColorLB.GetEntryColor(nPos).GetColor();
        else
            nNew= COL_TRANSPARENT;

        aAppOptions.SetTrackDeleteColor(nNew);

    }

    SC_MOD()->SetAppOptions(aAppOptions);

    //  Repaint (wenn alles ueber Items laufen wuerde, wie es sich gehoert,
    //  waere das nicht noetig...)
    ScDocShell* pDocSh = PTR_CAST(ScDocShell, SfxObjectShell::Current());
    if (pDocSh)
        pDocSh->PostPaintGridAll();

    return FALSE;
}

void __EXPORT ScRedlineOptionsTabPage::Reset( const SfxItemSet& /* rSet */ )
{

    XColorTable* pColorTbl = XColorTable::GetStdColorTable();
    aContentColorLB.InsertEntry(aAuthorStr);
    aMoveColorLB.InsertEntry(aAuthorStr);
    aInsertColorLB.InsertEntry(aAuthorStr);
    aRemoveColorLB.InsertEntry(aAuthorStr);

    aContentColorLB.SetUpdateMode( FALSE);
    aMoveColorLB.SetUpdateMode( FALSE);
    aInsertColorLB.SetUpdateMode( FALSE);
    aRemoveColorLB.SetUpdateMode( FALSE);

    for( USHORT i = 0; i < pColorTbl->Count(); ++i )
    {
        XColorEntry* pEntry = pColorTbl->GetColor( i );
        Color aColor = pEntry->GetColor();
        String sName = pEntry->GetName();

        aContentColorLB.InsertEntry( aColor, sName );
        aMoveColorLB.InsertEntry( aColor, sName );
        aInsertColorLB.InsertEntry( aColor, sName );
        aRemoveColorLB.InsertEntry( aColor, sName );
    }
    aContentColorLB.SetUpdateMode( TRUE );
    aMoveColorLB.SetUpdateMode( TRUE );
    aInsertColorLB.SetUpdateMode( TRUE );
    aRemoveColorLB.SetUpdateMode( TRUE );

    ScAppOptions aAppOptions=SC_MOD()->GetAppOptions();

    ULONG nColor = aAppOptions.GetTrackContentColor();
    if (nColor == COL_TRANSPARENT)
        aContentColorLB.SelectEntryPos(0);
    else
        aContentColorLB.SelectEntry(Color(nColor));

    nColor = aAppOptions.GetTrackMoveColor();
    if (nColor == COL_TRANSPARENT)
        aMoveColorLB.SelectEntryPos(0);
    else
        aMoveColorLB.SelectEntry(Color(nColor));

    nColor = aAppOptions.GetTrackInsertColor();
    if (nColor == COL_TRANSPARENT)
        aInsertColorLB.SelectEntryPos(0);
    else
        aInsertColorLB.SelectEntry(Color(nColor));

    nColor = aAppOptions.GetTrackDeleteColor();
    if (nColor == COL_TRANSPARENT)
        aRemoveColorLB.SelectEntryPos(0);
    else
        aRemoveColorLB.SelectEntry(Color(nColor));

}

IMPL_LINK( ScRedlineOptionsTabPage, ColorHdl, ColorListBox *, EMPTYARG )
{
/*
    SvxFontPrevWindow *pPrev;
    ListBox *pLB;

    if (pColorLB == &aInsertColorLB)
    {
        pPrev = &aInsertPreviewWN;
        pLB = &aInsertLB;
    }
    else
    {
        pPrev = &aDeletedPreviewWN;
        pLB = &aDeletedLB;
    }

    SvxFont& rFont = pPrev->GetFont();
    USHORT nPos = pLB->GetSelectEntryPos();
    if (nPos == LISTBOX_ENTRY_NOTFOUND)
        nPos = 0;

    CharAttr *pAttr = (CharAttr *)pLB->GetEntryData(nPos);

    if (pAttr->nItemId == SID_ATTR_BRUSH)
    {
        rFont.SetColor(Color(COL_BLACK));
        nPos = pColorLB->GetSelectEntryPos();
        if (nPos && nPos != LISTBOX_ENTRY_NOTFOUND)
        {
            Brush aBrush(Color(pColorLB->GetSelectEntryColor()));
            pPrev->SetBrush(aBrush);
        }
        else
        {
            Brush aBrush(Color(COL_LIGHTGRAY));
            pPrev->SetBrush(aBrush);
        }
    }
    else
    {
        nPos = pColorLB->GetSelectEntryPos();
        if (nPos && nPos != LISTBOX_ENTRY_NOTFOUND)
            rFont.SetColor(pColorLB->GetEntryColor(nPos));
        else
            rFont.SetColor(Color(COL_RED));
    }

    pPrev->Invalidate();
*/
    return 0;
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<|endoftext|>


<commit_before>#include <franka/model.h>

#include <sstream>

#include <research_interface/robot/service_types.h>

#include "model_library.h"
#include "network.h"
#include "robot_impl.h"

using namespace std::string_literals;  // NOLINT (google-build-using-namespace)

namespace franka {

Model::Model(Network& network) : library_{new ModelLibrary(network)} {}

// Has to be declared here, as the ModelLibrary type is incomplete in the
// header
Model::~Model() noexcept = default;

std::array<double, 16> Model::jointPose(Frame joint, const franka::RobotState& robot_state) const {
  std::array<double, 16> output;

  std::array<double, 7>::const_pointer q = robot_state.q.data();
  std::array<double, 16>::const_pointer end_effector = robot_state.O_T_EE.data();

  switch (joint) {
    case Frame::kJoint1:
      library_->joint1(q, end_effector, output.data());
      break;
    case Frame::kJoint2:
      library_->joint2(q, end_effector, output.data());
      break;
    case Frame::kJoint3:
      library_->joint3(q, end_effector, output.data());
      break;
    case Frame::kJoint4:
      library_->joint4(q, end_effector, output.data());
      break;
    case Frame::kJoint5:
      library_->joint5(q, end_effector, output.data());
      break;
    case Frame::kJoint6:
      library_->joint6(q, end_effector, output.data());
      break;
    case Frame::kJoint7:
      library_->joint7(q, end_effector, output.data());
      break;
    case Frame::kFlange:
      library_->flange(q, end_effector, output.data());
      break;
    case Frame::kEndEffector:
      library_->ee(q, end_effector, output.data());
      break;
    default:
      throw std::invalid_argument("Invalid joint given.");
  }

  return output;
}

std::array<double, 49> franka::Model::mass(
    const franka::RobotState& robot_state,
    const std::array<double, 9>& load_inertia,
    double load_mass,
    const std::array<double, 3>& F_x_Cload)  // NOLINT (readability-identifier-naming)
    const noexcept {
  std::array<double, 49> output;
  library_->mass(robot_state.q.data(), load_inertia.data(), load_mass, F_x_Cload.data(),
                 output.data());

  return output;
}

std::array<double, 7> franka::Model::coriolis(
    const franka::RobotState& robot_state,
    const std::array<double, 9>& load_inertia,
    double load_mass,
    const std::array<double, 3>& F_x_Cload)  // NOLINT (readability-identifier-naming)
    const noexcept {
  std::array<double, 7> output;
  library_->coriolis(robot_state.q.data(), robot_state.dq.data(), load_inertia.data(), load_mass,
                     F_x_Cload.data(), output.data());

  return output;
}

std::array<double, 7> franka::Model::gravity(
    const franka::RobotState& robot_state,
    double load_mass,
    const std::array<double, 3>& F_x_Cload,  // NOLINT (readability-identifier-naming)
    const std::array<double, 3>& gravity_earth) const noexcept {
  std::array<double, 7> output;
  library_->gravity(robot_state.q.data(), gravity_earth.data(), load_mass, F_x_Cload.data(),
                    output.data());

  return output;
}

}  // namespace franka
<commit_msg>remove redundant robot_impl.h include<commit_after>#include <franka/model.h>

#include <sstream>

#include <research_interface/robot/service_types.h>

#include "model_library.h"
#include "network.h"

using namespace std::string_literals;  // NOLINT (google-build-using-namespace)

namespace franka {

Model::Model(Network& network) : library_{new ModelLibrary(network)} {}

// Has to be declared here, as the ModelLibrary type is incomplete in the
// header
Model::~Model() noexcept = default;

std::array<double, 16> Model::jointPose(Frame joint, const franka::RobotState& robot_state) const {
  std::array<double, 16> output;

  std::array<double, 7>::const_pointer q = robot_state.q.data();
  std::array<double, 16>::const_pointer end_effector = robot_state.O_T_EE.data();

  switch (joint) {
    case Frame::kJoint1:
      library_->joint1(q, end_effector, output.data());
      break;
    case Frame::kJoint2:
      library_->joint2(q, end_effector, output.data());
      break;
    case Frame::kJoint3:
      library_->joint3(q, end_effector, output.data());
      break;
    case Frame::kJoint4:
      library_->joint4(q, end_effector, output.data());
      break;
    case Frame::kJoint5:
      library_->joint5(q, end_effector, output.data());
      break;
    case Frame::kJoint6:
      library_->joint6(q, end_effector, output.data());
      break;
    case Frame::kJoint7:
      library_->joint7(q, end_effector, output.data());
      break;
    case Frame::kFlange:
      library_->flange(q, end_effector, output.data());
      break;
    case Frame::kEndEffector:
      library_->ee(q, end_effector, output.data());
      break;
    default:
      throw std::invalid_argument("Invalid joint given.");
  }

  return output;
}

std::array<double, 49> franka::Model::mass(
    const franka::RobotState& robot_state,
    const std::array<double, 9>& load_inertia,
    double load_mass,
    const std::array<double, 3>& F_x_Cload)  // NOLINT (readability-identifier-naming)
    const noexcept {
  std::array<double, 49> output;
  library_->mass(robot_state.q.data(), load_inertia.data(), load_mass, F_x_Cload.data(),
                 output.data());

  return output;
}

std::array<double, 7> franka::Model::coriolis(
    const franka::RobotState& robot_state,
    const std::array<double, 9>& load_inertia,
    double load_mass,
    const std::array<double, 3>& F_x_Cload)  // NOLINT (readability-identifier-naming)
    const noexcept {
  std::array<double, 7> output;
  library_->coriolis(robot_state.q.data(), robot_state.dq.data(), load_inertia.data(), load_mass,
                     F_x_Cload.data(), output.data());

  return output;
}

std::array<double, 7> franka::Model::gravity(
    const franka::RobotState& robot_state,
    double load_mass,
    const std::array<double, 3>& F_x_Cload,  // NOLINT (readability-identifier-naming)
    const std::array<double, 3>& gravity_earth) const noexcept {
  std::array<double, 7> output;
  library_->gravity(robot_state.q.data(), gravity_earth.data(), load_mass, F_x_Cload.data(),
                    output.data());

  return output;
}

}  // namespace franka
<|endoftext|>


<commit_before>// The libMesh Finite Element Library.
// Copyright (C) 2002-2014 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// 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

// System Includes
#include <algorithm>
#include <iostream>

// Local Includes
#include "libmesh/libmesh_common.h"
#include "libmesh/parallel.h"
#include "libmesh/parallel_hilbert.h"
#include "libmesh/parallel_sort.h"
#include "libmesh/parallel_bin_sorter.h"
#ifdef LIBMESH_HAVE_LIBHILBERT
#  include "hilbert.h"
#endif

namespace libMesh
{

namespace Parallel {

// The Constructor sorts the local data using
// std::sort().  Therefore, the construction of
// a Parallel::Sort object takes O(nlogn) time,
// where n is the length of _data.
template <typename KeyType, typename IdxType>
Sort<KeyType,IdxType>::Sort(const Parallel::Communicator &comm_in,
                            std::vector<KeyType>& d) :
  ParallelObject(comm_in),
  _n_procs(cast_int<processor_id_type>(comm_in.size())),
  _proc_id(cast_int<processor_id_type>(comm_in.rank())),
  _bin_is_sorted(false),
  _data(d)
{
  std::sort(_data.begin(), _data.end());

  // Allocate storage
  _local_bin_sizes.resize(_n_procs);
}

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::sort()
{
  // Find the global data size.  The sorting
  // algorithms assume they have a range to
  // work with, so catch the degenerate cases here
  IdxType global_data_size = cast_int<IdxType>(_data.size());

  this->comm().sum (global_data_size);

  if (global_data_size < 2)
    {
      // the entire global range is either empty
      // or contains only one element
      _my_bin = _data;

      this->comm().allgather (static_cast<IdxType>(_my_bin.size()),
                              _local_bin_sizes);
    }
  else
    {
      if (this->n_processors() > 1)
        {
          this->binsort();
          this->communicate_bins();
        }
      else
        _my_bin = _data;

      this->sort_local_bin();
    }

  // Set sorted flag to true
  _bin_is_sorted = true;
}

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::binsort()
{
  // Find the global min and max from all the
  // processors.
  std::vector<KeyType> global_min_max(2);

  // Insert the local min and max for this processor
  global_min_max[0] = -_data.front();
  global_min_max[1] =  _data.back();

  // Communicate to determine the global
  // min and max for all processors.
  this->comm().max(global_min_max);

  // Multiply the min by -1 to obtain the true min
  global_min_max[0] *= -1;

  // Bin-Sort based on the global min and max
  Parallel::BinSorter<KeyType> bs(this->comm(), _data);
  bs.binsort(_n_procs, global_min_max[1], global_min_max[0]);

  // Now save the local bin sizes in a vector so
  // we don't have to keep around the BinSorter.
  for (processor_id_type i=0; i<_n_procs; ++i)
    _local_bin_sizes[i] = bs.sizeof_bin(i);
}

#if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)
// Full specialization for HilbertIndices, there is a fair amount of
// code duplication here that could potentially be consolidated with the
// above method
template <>
void Sort<Hilbert::HilbertIndices,unsigned int>::binsort()
{
  // Find the global min and max from all the
  // processors.  Do this using MPI_Allreduce.
  Hilbert::HilbertIndices
    local_min,  local_max,
    global_min, global_max;

  if (_data.empty())
    {
      local_min.rack0 = local_min.rack1 = local_min.rack2 = static_cast<Hilbert::inttype>(-1);
      local_max.rack0 = local_max.rack1 = local_max.rack2 = 0;
    }
  else
    {
      local_min = _data.front();
      local_max = _data.back();
    }

  MPI_Op hilbert_max, hilbert_min;

  MPI_Op_create       ((MPI_User_function*)__hilbert_max_op, true, &hilbert_max);
  MPI_Op_create       ((MPI_User_function*)__hilbert_min_op, true, &hilbert_min);

  // Communicate to determine the global
  // min and max for all processors.
  MPI_Allreduce(&local_min,
                &global_min,
                1,
                Parallel::StandardType<Hilbert::HilbertIndices>(),
                hilbert_min,
                this->comm().get());

  MPI_Allreduce(&local_max,
                &global_max,
                1,
                Parallel::StandardType<Hilbert::HilbertIndices>(),
                hilbert_max,
                this->comm().get());

  MPI_Op_free   (&hilbert_max);
  MPI_Op_free   (&hilbert_min);

  // Bin-Sort based on the global min and max
  Parallel::BinSorter<Hilbert::HilbertIndices> bs(this->comm(),_data);
  bs.binsort(_n_procs, global_max, global_min);

  // Now save the local bin sizes in a vector so
  // we don't have to keep around the BinSorter.
  for (processor_id_type i=0; i<_n_procs; ++i)
    _local_bin_sizes[i] = bs.sizeof_bin(i);
}

#endif // #ifdef LIBMESH_HAVE_LIBHILBERT

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::communicate_bins()
{
#ifdef LIBMESH_HAVE_MPI
  // Create storage for the global bin sizes.  This
  // is the number of keys which will be held in
  // each bin over all processors.
  std::vector<IdxType> global_bin_sizes = _local_bin_sizes;

  // Sum to find the total number of entries in each bin.
  this->comm().sum(global_bin_sizes);

  // Create a vector to temporarily hold the results of MPI_Gatherv
  // calls.  The vector dest  may be saved away to _my_bin depending on which
  // processor is being MPI_Gatherv'd.
  std::vector<KeyType> dest;

  IdxType local_offset = 0;

  for (processor_id_type i=0; i<_n_procs; ++i)
    {
      // Vector to receive the total bin size for each
      // processor.  Processor i's bin size will be
      // held in proc_bin_size[i]
      std::vector<int> proc_bin_size;

      // Find the number of contributions coming from each
      // processor for this bin.  Note: allgather combines
      // the MPI_Gather and MPI_Bcast operations into one.
      this->comm().allgather(static_cast<int>(_local_bin_sizes[i]),
                             proc_bin_size);

      // Compute the offsets into my_bin for each processor's
      // portion of the bin.  These are basically partial sums
      // of the proc_bin_size vector.
      std::vector<int> displacements(_n_procs);
      for (processor_id_type j=1; j<_n_procs; ++j)
        displacements[j] = proc_bin_size[j-1] + displacements[j-1];

      // Resize the destination buffer
      dest.resize (global_bin_sizes[i]);

      MPI_Gatherv((_data.size() > local_offset) ?
                  &_data[local_offset] :
                  NULL,                            // Points to the beginning of the bin to be sent
                  _local_bin_sizes[i],               // How much data is in the bin being sent.
                  Parallel::StandardType<KeyType>(), // The data type we are sorting
                  (dest.empty()) ?
                  NULL :
                  &dest[0],                        // Enough storage to hold all bin contributions
                  &proc_bin_size[0],          // How much is to be received from each processor
                  &displacements[0],          // Offsets into the receive buffer
                  Parallel::StandardType<KeyType>(), // The data type we are sorting
                  i,                                 // The root process (we do this once for each proc)
                  this->comm().get());

      // Copy the destination buffer if it
      // corresponds to the bin for this processor
      if (i == _proc_id)
        _my_bin = dest;

      // Increment the local offset counter
      local_offset += _local_bin_sizes[i];
    }
#endif // LIBMESH_HAVE_MPI
}

#if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)
// Full specialization for HilbertIndices, there is a fair amount of
// code duplication here that could potentially be consolidated with the
// above method
template <>
void Sort<Hilbert::HilbertIndices,unsigned int>::communicate_bins()
{
  // Create storage for the global bin sizes.  This
  // is the number of keys which will be held in
  // each bin over all processors.
  std::vector<unsigned int> global_bin_sizes(_n_procs);

  libmesh_assert_equal_to (_local_bin_sizes.size(), global_bin_sizes.size());

  // Sum to find the total number of entries in each bin.
  // This is stored in global_bin_sizes.  Note, we
  // explicitly know that we are communicating MPI_UNSIGNED's here.
  MPI_Allreduce(&_local_bin_sizes[0],
                &global_bin_sizes[0],
                _n_procs,
                MPI_UNSIGNED,
                MPI_SUM,
                this->comm().get());

  // Create a vector to temporarily hold the results of MPI_Gatherv
  // calls.  The vector dest  may be saved away to _my_bin depending on which
  // processor is being MPI_Gatherv'd.
  std::vector<Hilbert::HilbertIndices> dest;

  unsigned int local_offset = 0;

  for (unsigned int i=0; i<_n_procs; ++i)
    {
      // Vector to receive the total bin size for each
      // processor.  Processor i's bin size will be
      // held in proc_bin_size[i]
      std::vector<int> proc_bin_size(_n_procs);

      // Find the number of contributions coming from each
      // processor for this bin.  Note: Allgather combines
      // the MPI_Gather and MPI_Bcast operations into one.
      // Note: Here again we know that we are communicating
      // MPI_UNSIGNED's so there is no need to check the MPI_traits.
      MPI_Allgather(&_local_bin_sizes[i], // Source: # of entries on this proc in bin i
                    1,                    // Number of items to gather
                    MPI_UNSIGNED,
                    &proc_bin_size[0],    // Destination: Total # of entries in bin i
                    1,
                    MPI_INT,
                    this->comm().get());

      // Compute the offsets into my_bin for each processor's
      // portion of the bin.  These are basically partial sums
      // of the proc_bin_size vector.
      std::vector<int> displacements(_n_procs);
      for (unsigned int j=1; j<_n_procs; ++j)
        displacements[j] = proc_bin_size[j-1] + displacements[j-1];

      // Resize the destination buffer
      dest.resize (global_bin_sizes[i]);

      MPI_Gatherv((_data.size() > local_offset) ?
                  &_data[local_offset] :
                  NULL,                   // Points to the beginning of the bin to be sent
                  _local_bin_sizes[i],      // How much data is in the bin being sent.
                  Parallel::StandardType<Hilbert::HilbertIndices>(), // The data type we are sorting
                  (dest.empty()) ?
                  NULL :
                  &dest[0],               // Enough storage to hold all bin contributions
                  &proc_bin_size[0], // How much is to be received from each processor
                  &displacements[0], // Offsets into the receive buffer
                  Parallel::StandardType<Hilbert::HilbertIndices>(), // The data type we are sorting
                  i,                        // The root process (we do this once for each proc)
                  this->comm().get());

      // Copy the destination buffer if it
      // corresponds to the bin for this processor
      if (i == _proc_id)
        _my_bin = dest;

      // Increment the local offset counter
      local_offset += _local_bin_sizes[i];
    }
}

#endif // #if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::sort_local_bin()
{
  std::sort(_my_bin.begin(), _my_bin.end());
}

template <typename KeyType, typename IdxType>
const std::vector<KeyType>& Sort<KeyType,IdxType>::bin()
{
  if (!_bin_is_sorted)
    {
      libMesh::out << "Warning! Bin is not yet sorted!" << std::endl;
    }

  return _my_bin;
}

}

// Explicitly instantiate for int, double
template class Parallel::Sort<int, unsigned int>;
template class Parallel::Sort<double, unsigned int>;
#if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)
template class Parallel::Sort<Hilbert::HilbertIndices, unsigned int>;
#endif

} // namespace libMesh
<commit_msg>Construct the sendbuf/recvbuf pointers separately to simplify debugging.<commit_after>// The libMesh Finite Element Library.
// Copyright (C) 2002-2014 Benjamin S. Kirk, John W. Peterson, Roy H. Stogner

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// 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

// System Includes
#include <algorithm>
#include <iostream>

// Local Includes
#include "libmesh/libmesh_common.h"
#include "libmesh/parallel.h"
#include "libmesh/parallel_hilbert.h"
#include "libmesh/parallel_sort.h"
#include "libmesh/parallel_bin_sorter.h"
#ifdef LIBMESH_HAVE_LIBHILBERT
#  include "hilbert.h"
#endif

namespace libMesh
{

namespace Parallel {

// The Constructor sorts the local data using
// std::sort().  Therefore, the construction of
// a Parallel::Sort object takes O(nlogn) time,
// where n is the length of _data.
template <typename KeyType, typename IdxType>
Sort<KeyType,IdxType>::Sort(const Parallel::Communicator &comm_in,
                            std::vector<KeyType>& d) :
  ParallelObject(comm_in),
  _n_procs(cast_int<processor_id_type>(comm_in.size())),
  _proc_id(cast_int<processor_id_type>(comm_in.rank())),
  _bin_is_sorted(false),
  _data(d)
{
  std::sort(_data.begin(), _data.end());

  // Allocate storage
  _local_bin_sizes.resize(_n_procs);
}

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::sort()
{
  // Find the global data size.  The sorting
  // algorithms assume they have a range to
  // work with, so catch the degenerate cases here
  IdxType global_data_size = cast_int<IdxType>(_data.size());

  this->comm().sum (global_data_size);

  if (global_data_size < 2)
    {
      // the entire global range is either empty
      // or contains only one element
      _my_bin = _data;

      this->comm().allgather (static_cast<IdxType>(_my_bin.size()),
                              _local_bin_sizes);
    }
  else
    {
      if (this->n_processors() > 1)
        {
          this->binsort();
          this->communicate_bins();
        }
      else
        _my_bin = _data;

      this->sort_local_bin();
    }

  // Set sorted flag to true
  _bin_is_sorted = true;
}

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::binsort()
{
  // Find the global min and max from all the
  // processors.
  std::vector<KeyType> global_min_max(2);

  // Insert the local min and max for this processor
  global_min_max[0] = -_data.front();
  global_min_max[1] =  _data.back();

  // Communicate to determine the global
  // min and max for all processors.
  this->comm().max(global_min_max);

  // Multiply the min by -1 to obtain the true min
  global_min_max[0] *= -1;

  // Bin-Sort based on the global min and max
  Parallel::BinSorter<KeyType> bs(this->comm(), _data);
  bs.binsort(_n_procs, global_min_max[1], global_min_max[0]);

  // Now save the local bin sizes in a vector so
  // we don't have to keep around the BinSorter.
  for (processor_id_type i=0; i<_n_procs; ++i)
    _local_bin_sizes[i] = bs.sizeof_bin(i);
}

#if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)
// Full specialization for HilbertIndices, there is a fair amount of
// code duplication here that could potentially be consolidated with the
// above method
template <>
void Sort<Hilbert::HilbertIndices,unsigned int>::binsort()
{
  // Find the global min and max from all the
  // processors.  Do this using MPI_Allreduce.
  Hilbert::HilbertIndices
    local_min,  local_max,
    global_min, global_max;

  if (_data.empty())
    {
      local_min.rack0 = local_min.rack1 = local_min.rack2 = static_cast<Hilbert::inttype>(-1);
      local_max.rack0 = local_max.rack1 = local_max.rack2 = 0;
    }
  else
    {
      local_min = _data.front();
      local_max = _data.back();
    }

  MPI_Op hilbert_max, hilbert_min;

  MPI_Op_create       ((MPI_User_function*)__hilbert_max_op, true, &hilbert_max);
  MPI_Op_create       ((MPI_User_function*)__hilbert_min_op, true, &hilbert_min);

  // Communicate to determine the global
  // min and max for all processors.
  MPI_Allreduce(&local_min,
                &global_min,
                1,
                Parallel::StandardType<Hilbert::HilbertIndices>(),
                hilbert_min,
                this->comm().get());

  MPI_Allreduce(&local_max,
                &global_max,
                1,
                Parallel::StandardType<Hilbert::HilbertIndices>(),
                hilbert_max,
                this->comm().get());

  MPI_Op_free   (&hilbert_max);
  MPI_Op_free   (&hilbert_min);

  // Bin-Sort based on the global min and max
  Parallel::BinSorter<Hilbert::HilbertIndices> bs(this->comm(),_data);
  bs.binsort(_n_procs, global_max, global_min);

  // Now save the local bin sizes in a vector so
  // we don't have to keep around the BinSorter.
  for (processor_id_type i=0; i<_n_procs; ++i)
    _local_bin_sizes[i] = bs.sizeof_bin(i);
}

#endif // #ifdef LIBMESH_HAVE_LIBHILBERT

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::communicate_bins()
{
#ifdef LIBMESH_HAVE_MPI
  // Create storage for the global bin sizes.  This
  // is the number of keys which will be held in
  // each bin over all processors.
  std::vector<IdxType> global_bin_sizes = _local_bin_sizes;

  // Sum to find the total number of entries in each bin.
  this->comm().sum(global_bin_sizes);

  // Create a vector to temporarily hold the results of MPI_Gatherv
  // calls.  The vector dest  may be saved away to _my_bin depending on which
  // processor is being MPI_Gatherv'd.
  std::vector<KeyType> dest;

  IdxType local_offset = 0;

  for (processor_id_type i=0; i<_n_procs; ++i)
    {
      // Vector to receive the total bin size for each
      // processor.  Processor i's bin size will be
      // held in proc_bin_size[i]
      std::vector<int> proc_bin_size;

      // Find the number of contributions coming from each
      // processor for this bin.  Note: allgather combines
      // the MPI_Gather and MPI_Bcast operations into one.
      this->comm().allgather(static_cast<int>(_local_bin_sizes[i]),
                             proc_bin_size);

      // Compute the offsets into my_bin for each processor's
      // portion of the bin.  These are basically partial sums
      // of the proc_bin_size vector.
      std::vector<int> displacements(_n_procs);
      for (processor_id_type j=1; j<_n_procs; ++j)
        displacements[j] = proc_bin_size[j-1] + displacements[j-1];

      // Resize the destination buffer
      dest.resize (global_bin_sizes[i]);

      // Points to the beginning of the bin to be sent
      void * sendbuf = (_data.size() > local_offset) ? &_data[local_offset] : NULL;

      // Enough storage to hold all bin contributions
      void * recvbuf = (dest.empty()) ? NULL : &dest[0];

      // If the sendbuf is NULL, make sure we aren't claiming to send something.
      if (sendbuf == NULL && _local_bin_sizes[i] != 0)
        libmesh_error_msg("Error: invalid MPI_Gatherv call constructed!");

      MPI_Gatherv(sendbuf,
                  _local_bin_sizes[i],               // How much data is in the bin being sent.
                  Parallel::StandardType<KeyType>(), // The data type we are sorting
                  recvbuf,
                  &proc_bin_size[0],          // How much is to be received from each processor
                  &displacements[0],          // Offsets into the receive buffer
                  Parallel::StandardType<KeyType>(), // The data type we are sorting
                  i,                                 // The root process (we do this once for each proc)
                  this->comm().get());

      // Copy the destination buffer if it
      // corresponds to the bin for this processor
      if (i == _proc_id)
        _my_bin = dest;

      // Increment the local offset counter
      local_offset += _local_bin_sizes[i];
    }
#endif // LIBMESH_HAVE_MPI
}

#if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)
// Full specialization for HilbertIndices, there is a fair amount of
// code duplication here that could potentially be consolidated with the
// above method
template <>
void Sort<Hilbert::HilbertIndices,unsigned int>::communicate_bins()
{
  // Create storage for the global bin sizes.  This
  // is the number of keys which will be held in
  // each bin over all processors.
  std::vector<unsigned int> global_bin_sizes(_n_procs);

  libmesh_assert_equal_to (_local_bin_sizes.size(), global_bin_sizes.size());

  // Sum to find the total number of entries in each bin.
  // This is stored in global_bin_sizes.  Note, we
  // explicitly know that we are communicating MPI_UNSIGNED's here.
  MPI_Allreduce(&_local_bin_sizes[0],
                &global_bin_sizes[0],
                _n_procs,
                MPI_UNSIGNED,
                MPI_SUM,
                this->comm().get());

  // Create a vector to temporarily hold the results of MPI_Gatherv
  // calls.  The vector dest  may be saved away to _my_bin depending on which
  // processor is being MPI_Gatherv'd.
  std::vector<Hilbert::HilbertIndices> dest;

  unsigned int local_offset = 0;

  for (unsigned int i=0; i<_n_procs; ++i)
    {
      // Vector to receive the total bin size for each
      // processor.  Processor i's bin size will be
      // held in proc_bin_size[i]
      std::vector<int> proc_bin_size(_n_procs);

      // Find the number of contributions coming from each
      // processor for this bin.  Note: Allgather combines
      // the MPI_Gather and MPI_Bcast operations into one.
      // Note: Here again we know that we are communicating
      // MPI_UNSIGNED's so there is no need to check the MPI_traits.
      MPI_Allgather(&_local_bin_sizes[i], // Source: # of entries on this proc in bin i
                    1,                    // Number of items to gather
                    MPI_UNSIGNED,
                    &proc_bin_size[0],    // Destination: Total # of entries in bin i
                    1,
                    MPI_INT,
                    this->comm().get());

      // Compute the offsets into my_bin for each processor's
      // portion of the bin.  These are basically partial sums
      // of the proc_bin_size vector.
      std::vector<int> displacements(_n_procs);
      for (unsigned int j=1; j<_n_procs; ++j)
        displacements[j] = proc_bin_size[j-1] + displacements[j-1];

      // Resize the destination buffer
      dest.resize (global_bin_sizes[i]);

      // Points to the beginning of the bin to be sent
      void * sendbuf = (_data.size() > local_offset) ? &_data[local_offset] : NULL;

      // Enough storage to hold all bin contributions
      void * recvbuf = (dest.empty()) ? NULL : &dest[0];

      // If the sendbuf is NULL, make sure we aren't claiming to send something.
      if (sendbuf == NULL && _local_bin_sizes[i] != 0)
        libmesh_error_msg("Error: invalid MPI_Gatherv call constructed!");

      MPI_Gatherv(sendbuf,
                  _local_bin_sizes[i],      // How much data is in the bin being sent.
                  Parallel::StandardType<Hilbert::HilbertIndices>(), // The data type we are sorting
                  recvbuf,
                  &proc_bin_size[0], // How much is to be received from each processor
                  &displacements[0], // Offsets into the receive buffer
                  Parallel::StandardType<Hilbert::HilbertIndices>(), // The data type we are sorting
                  i,                        // The root process (we do this once for each proc)
                  this->comm().get());

      // Copy the destination buffer if it
      // corresponds to the bin for this processor
      if (i == _proc_id)
        _my_bin = dest;

      // Increment the local offset counter
      local_offset += _local_bin_sizes[i];
    }
}

#endif // #if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)

template <typename KeyType, typename IdxType>
void Sort<KeyType,IdxType>::sort_local_bin()
{
  std::sort(_my_bin.begin(), _my_bin.end());
}

template <typename KeyType, typename IdxType>
const std::vector<KeyType>& Sort<KeyType,IdxType>::bin()
{
  if (!_bin_is_sorted)
    {
      libMesh::out << "Warning! Bin is not yet sorted!" << std::endl;
    }

  return _my_bin;
}

}

// Explicitly instantiate for int, double
template class Parallel::Sort<int, unsigned int>;
template class Parallel::Sort<double, unsigned int>;
#if defined(LIBMESH_HAVE_LIBHILBERT) && defined(LIBMESH_HAVE_MPI)
template class Parallel::Sort<Hilbert::HilbertIndices, unsigned int>;
#endif

} // namespace libMesh
<|endoftext|>


<commit_before>#include "cnn/training.h"

// #include "cnn/gpu-ops.h"
#include "cnn/param-nodes.h"
#include "cnn/weight-decay.h"

// Macros for defining parameter update functions
#ifdef __CUDACC__
#define CNN_TRAINER_INST_DEV_IMPL(MyTrainer) \
  template void MyTrainer::update_rule_dev<Device_GPU>(const Device_GPU & dev, real scale, real gscale, const std::vector<Tensor*> & values);
#elif defined(HAVE_CUDA)
#define CNN_TRAINER_INST_DEV_IMPL(MyTrainer) \
  extern template void MyTrainer::update_rule_dev<Device_GPU>(const Device_GPU & dev, real scale, real gscale, const std::vector<Tensor*> & values); \
  template void MyTrainer::update_rule_dev<Device_CPU>(const Device_CPU & dev, real scale, real gscale, const std::vector<Tensor*> & values); \
  void MyTrainer::update_rule(real scale, real gscale, const std::vector<Tensor*> & values) { \
    if(values[0]->device->type == DeviceType::CPU) { update_rule_dev(*(Device_CPU*)values[0]->device,scale,gscale,values); } \
    else if(values[0]->device->type == DeviceType::GPU) { update_rule_dev(*(Device_GPU*)values[0]->device,scale,gscale,values); } \
    else { abort(); } \
  }
#else
#define CNN_TRAINER_INST_DEV_IMPL(MyTrainer) \
  template void MyTrainer::update_rule_dev<Device_CPU>(const Device_CPU & dev, real scale, real gscale, const std::vector<Tensor*> & values); \
  void MyTrainer::update_rule(real scale, real gscale, const std::vector<Tensor*> & values) { \
    if(values[0]->device->type == DeviceType::CPU) { update_rule_dev(*(Device_CPU*)values[0]->device,scale,gscale,values); } \
    else { abort(); } \
  }
#endif

namespace cnn {

using namespace std;

template <class Derived>
bool is_valid(const Eigen::MatrixBase<Derived>& x) {
  return ((x - x).array() == (x - x).array()).all();
}

// --- The actual update code for each operation, implemented on various devices

// Trainer base class is run on CPUs
#ifndef __CUDACC__

Trainer::~Trainer() {}

void Trainer::rescale_and_reset_weight_decay() {
  const float weight_decay = global_weight_decay.CurrentWeightDecay();
  for (auto p : model->parameters_list())
    p->scale_parameters(weight_decay);
  global_weight_decay.ResetWeightDecay();
}

float Trainer::clip_gradients() {
  float gscale = 1;
  if (clipping_enabled) {
    float gg = model->gradient_l2_norm();
    if (isnan(gg) || isinf(gg)) {
      cerr << "Magnitude of gradient is bad: " << gg << endl;
      abort();
    }
    if (gg > clip_threshold) {
      ++clips;
      gscale = clip_threshold / gg;
    }
  }
  return gscale;
}

// this calls the rule-specific
void Trainer::update(real scale) {
  // Allocate if necessary
  if(!aux_allocated) {
    alloc_impl();
    aux_allocated = true;
  }

  // Perform gradient clipping and cycle through parameters
  const float gscale = clip_gradients();
  const auto & params = model->parameters_list();
  for(size_t i = 0; i < params.size(); ++i) {
    update_params(scale, gscale, i);
    params[i]->clear();
  }
  const auto & lookup_params = model->lookup_parameters_list();
  for(size_t i = 0; i < lookup_params.size(); ++i) {
    for (auto j : lookup_params[i]->non_zero_grads)
      update_lookup_params(scale, gscale, i, j);
    lookup_params[i]->clear();
  }
  ++updates;

  global_weight_decay.UpdateWeightDecay(); // update global weight scale
  if (global_weight_decay.ParametersNeedRescaled())
    rescale_and_reset_weight_decay();  // if wdscale is getting to small multiply all weights by wdscale, and set wdscale to 1
}

#endif

// --- SimpleSGDTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients
template <class MyDevice>
void SimpleSGDTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[0]->tvec().device(*dev.edevice) -= ts[1]->tvec() * (eta * scale * gscale / global_weight_decay.CurrentWeightDecay());
}
CNN_TRAINER_INST_DEV_IMPL(SimpleSGDTrainer)

#ifndef __CUDACC__
void SimpleSGDTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g});
}
void SimpleSGDTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx]});
}
#endif

// --- MomentumSGDTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=momentum
template <class MyDevice>
void MomentumSGDTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[2]->tvec().device(*dev.edevice) = ts[2]->tvec() * momentum - ts[1]->tvec() * (eta * scale * gscale);
  ts[0]->tvec().device(*dev.edevice) += ts[2]->tvec() / global_weight_decay.CurrentWeightDecay();
}
CNN_TRAINER_INST_DEV_IMPL(MomentumSGDTrainer)

#ifndef __CUDACC__
void MomentumSGDTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &vp[idx].h});
}
void MomentumSGDTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &vlp[idx].h[lidx]});
}
void MomentumSGDTrainer::alloc_impl() {
  vp = AllocateShadowParameters(*model);
  vlp = AllocateShadowLookupParameters(*model);
}
#endif

// --- AdagradTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=stddev
template <class MyDevice>
void AdagradTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[1]->tvec().device(*dev.edevice) = ts[1]->tvec() * (scale * gscale);
  ts[2]->tvec().device(*dev.edevice) += ts[1]->tvec().square();
  ts[0]->tvec().device(*dev.edevice) += ts[1]->tvec() / (ts[2]->tvec() + epsilon).sqrt() * (-eta / global_weight_decay.CurrentWeightDecay());
}
CNN_TRAINER_INST_DEV_IMPL(AdagradTrainer)

#ifndef __CUDACC__
void AdagradTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &vp[idx].h});
}
void AdagradTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &vlp[idx].h[lidx]});
}
void AdagradTrainer::alloc_impl() {
  vp = AllocateShadowParameters(*model);
  vlp = AllocateShadowLookupParameters(*model);
}
#endif

// --- AdadeltaTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=hg, ts[3]=hd
template <class MyDevice>
void AdadeltaTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[1]->tvec().device(*dev.edevice) = ts[1]->tvec() * (scale * gscale);
  ts[2]->tvec().device(*dev.edevice) = ts[2]->tvec() * rho + ts[1]->tvec().square() * (1.f - rho);
  ts[1]->tvec().device(*dev.edevice) = - ts[1]->tvec() * (ts[3]->tvec() + epsilon).sqrt() / (ts[2]->tvec() + epsilon).sqrt();
  ts[3]->tvec().device(*dev.edevice) = ts[3]->tvec() * rho + ts[1]->tvec().square() * (1.f - rho);
  ts[0]->tvec().device(*dev.edevice) += ts[1]->tvec() / global_weight_decay.CurrentWeightDecay();
}
CNN_TRAINER_INST_DEV_IMPL(AdadeltaTrainer)

#ifndef __CUDACC__
void AdadeltaTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &hg[idx].h, &hd[idx].h});
}
void AdadeltaTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &hlg[idx].h[lidx], &hld[idx].h[lidx]});
}
void AdadeltaTrainer::alloc_impl() {
  hg = AllocateShadowParameters(*model);
  hlg = AllocateShadowLookupParameters(*model);
  hd = AllocateShadowParameters(*model);
  hld = AllocateShadowLookupParameters(*model);
}
#endif

// --- RmsPropTrainer
// TODO: This is not finished yet, because it memorizes a scalar for each set of parameters, not each parameter itself.
//       We could implement this with one tensor for each scalar, but this is pretty wasteful

// Perform update of ts[0]=parameters, ts[1]=gradients
template <class MyDevice>
void RmsPropTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // real& d2 = hg[pi++];
  // real g2 = p->g.vec().squaredNorm();
  // d2 = rho * d2 + (1.f - rho) * g2;
  // p->values.vec() -= ((eta * scale * gscale / sqrt(d2 + epsilon)) * p->g.vec()) / global_weight_decay.CurrentWeightDecay();
}
CNN_TRAINER_INST_DEV_IMPL(RmsPropTrainer)

#ifndef __CUDACC__
void RmsPropTrainer::update_params(real scale, real gscale, size_t idx) {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // auto & p = model->parameters_list()[idx];
  // update_rule(scale, gscale, {&p->values, &p->g, &hg[idx].h, &hd[idx].h});
}
void RmsPropTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // auto & p = model->lookup_parameters_list()[idx];
  // update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &hlg[idx].h[lidx], &hld[idx].h[lidx]});
}
void RmsPropTrainer::alloc_impl() {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // hg.resize(model->parameters_list().size());
  // unsigned pi = 0;
  // hlg.resize(model->lookup_parameters_list().size());
  // for (auto p : model->lookup_parameters_list()) {
  //   hlg[pi++].resize(p->size());
  // }
}
#endif

// --- AdamTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=mean, ts[3]=variance
template <class MyDevice>
void AdamTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[1]->tvec().device(*dev.edevice) = ts[1]->tvec() * (scale * gscale);
  ts[2]->tvec().device(*dev.edevice) = ts[2]->tvec() * beta_1 + ts[1]->tvec() * (1.f - beta_1);
  ts[3]->tvec().device(*dev.edevice) = ts[3]->tvec() * beta_2 + ts[1]->tvec().square() * (1.f - beta_2);
  float s1 = 1 - pow(beta_1, updates+1);
  float s2 = 1 - pow(beta_2, updates+1);
  ts[0]->tvec().device(*dev.edevice) += ts[2]->tvec() / ((ts[3]->tvec() / s2).sqrt() + epsilon) * (-eta / s1 / global_weight_decay.CurrentWeightDecay());
}
CNN_TRAINER_INST_DEV_IMPL(AdamTrainer)

#ifndef __CUDACC__
void AdamTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &m[idx].h, &v[idx].h});
}
void AdamTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &lm[idx].h[lidx], &lv[idx].h[lidx]});
}
void AdamTrainer::alloc_impl() {
  m = AllocateShadowParameters(*model);
  lm = AllocateShadowLookupParameters(*model);
  v = AllocateShadowParameters(*model);
  lv = AllocateShadowLookupParameters(*model);
}
#endif

} // namespace cnn
<commit_msg>Temporary fix of segfaults when training loaded models<commit_after>#include "cnn/training.h"

// #include "cnn/gpu-ops.h"
#include "cnn/param-nodes.h"
#include "cnn/weight-decay.h"

// Macros for defining parameter update functions
#ifdef __CUDACC__
#define CNN_TRAINER_INST_DEV_IMPL(MyTrainer) \
  template void MyTrainer::update_rule_dev<Device_GPU>(const Device_GPU & dev, real scale, real gscale, const std::vector<Tensor*> & values);
#elif defined(HAVE_CUDA)
// This is correct, but dying when models are read and written.
// if(values[0]->device->type == DeviceType::CPU) { update_rule_dev(*(Device_CPU*)values[0]->device,scale,gscale,values); }
// else if(values[0]->device->type == DeviceType::GPU) { update_rule_dev(*(Device_GPU*)values[0]->device,scale,gscale,values); }
// else { abort(); }
#define CNN_TRAINER_INST_DEV_IMPL(MyTrainer) \
  extern template void MyTrainer::update_rule_dev<Device_GPU>(const Device_GPU & dev, real scale, real gscale, const std::vector<Tensor*> & values); \
  template void MyTrainer::update_rule_dev<Device_CPU>(const Device_CPU & dev, real scale, real gscale, const std::vector<Tensor*> & values); \
  void MyTrainer::update_rule(real scale, real gscale, const std::vector<Tensor*> & values) { \
    if(default_device->type == DeviceType::CPU) { update_rule_dev(*(Device_CPU*)default_device,scale,gscale,values); } \
    else if(default_device->type == DeviceType::GPU) { update_rule_dev(*(Device_GPU*)default_device,scale,gscale,values); } \
    else { abort(); } \
  }
#else
#define CNN_TRAINER_INST_DEV_IMPL(MyTrainer) \
  template void MyTrainer::update_rule_dev<Device_CPU>(const Device_CPU & dev, real scale, real gscale, const std::vector<Tensor*> & values); \
  void MyTrainer::update_rule(real scale, real gscale, const std::vector<Tensor*> & values) { \
    if(default_device->type == DeviceType::CPU) { update_rule_dev(*(Device_CPU*)default_device,scale,gscale,values); } \
    else { abort(); } \
  }
#endif

namespace cnn {

using namespace std;

template <class Derived>
bool is_valid(const Eigen::MatrixBase<Derived>& x) {
  return ((x - x).array() == (x - x).array()).all();
}

// --- The actual update code for each operation, implemented on various devices

// Trainer base class is run on CPUs
#ifndef __CUDACC__

Trainer::~Trainer() {}

void Trainer::rescale_and_reset_weight_decay() {
  const float weight_decay = global_weight_decay.CurrentWeightDecay();
  for (auto p : model->parameters_list())
    p->scale_parameters(weight_decay);
  global_weight_decay.ResetWeightDecay();
}

float Trainer::clip_gradients() {
  float gscale = 1;
  if (clipping_enabled) {
    float gg = model->gradient_l2_norm();
    if (isnan(gg) || isinf(gg)) {
      cerr << "Magnitude of gradient is bad: " << gg << endl;
      abort();
    }
    if (gg > clip_threshold) {
      ++clips;
      gscale = clip_threshold / gg;
    }
  }
  return gscale;
}

// this calls the rule-specific
void Trainer::update(real scale) {
  // Allocate if necessary
  if(!aux_allocated) {
    alloc_impl();
    aux_allocated = true;
  }

  // Perform gradient clipping and cycle through parameters
  const float gscale = clip_gradients();
  const auto & params = model->parameters_list();
  for(size_t i = 0; i < params.size(); ++i) {
    update_params(scale, gscale, i);
    params[i]->clear();
  }
  const auto & lookup_params = model->lookup_parameters_list();
  for(size_t i = 0; i < lookup_params.size(); ++i) {
    for (auto j : lookup_params[i]->non_zero_grads)
      update_lookup_params(scale, gscale, i, j);
    lookup_params[i]->clear();
  }
  ++updates;

  global_weight_decay.UpdateWeightDecay(); // update global weight scale
  if (global_weight_decay.ParametersNeedRescaled())
    rescale_and_reset_weight_decay();  // if wdscale is getting to small multiply all weights by wdscale, and set wdscale to 1
}

#endif

// --- SimpleSGDTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients
template <class MyDevice>
void SimpleSGDTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[0]->tvec().device(*dev.edevice) -= ts[1]->tvec() * (eta * scale * gscale / global_weight_decay.CurrentWeightDecay());
}
CNN_TRAINER_INST_DEV_IMPL(SimpleSGDTrainer)

#ifndef __CUDACC__
void SimpleSGDTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g});
}
void SimpleSGDTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx]});
}
#endif

// --- MomentumSGDTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=momentum
template <class MyDevice>
void MomentumSGDTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[2]->tvec().device(*dev.edevice) = ts[2]->tvec() * momentum - ts[1]->tvec() * (eta * scale * gscale);
  ts[0]->tvec().device(*dev.edevice) += ts[2]->tvec() / global_weight_decay.CurrentWeightDecay();
}
CNN_TRAINER_INST_DEV_IMPL(MomentumSGDTrainer)

#ifndef __CUDACC__
void MomentumSGDTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &vp[idx].h});
}
void MomentumSGDTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &vlp[idx].h[lidx]});
}
void MomentumSGDTrainer::alloc_impl() {
  vp = AllocateShadowParameters(*model);
  vlp = AllocateShadowLookupParameters(*model);
}
#endif

// --- AdagradTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=stddev
template <class MyDevice>
void AdagradTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[1]->tvec().device(*dev.edevice) = ts[1]->tvec() * (scale * gscale);
  ts[2]->tvec().device(*dev.edevice) += ts[1]->tvec().square();
  ts[0]->tvec().device(*dev.edevice) += ts[1]->tvec() / (ts[2]->tvec() + epsilon).sqrt() * (-eta / global_weight_decay.CurrentWeightDecay());
}
CNN_TRAINER_INST_DEV_IMPL(AdagradTrainer)

#ifndef __CUDACC__
void AdagradTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &vp[idx].h});
}
void AdagradTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &vlp[idx].h[lidx]});
}
void AdagradTrainer::alloc_impl() {
  vp = AllocateShadowParameters(*model);
  vlp = AllocateShadowLookupParameters(*model);
}
#endif

// --- AdadeltaTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=hg, ts[3]=hd
template <class MyDevice>
void AdadeltaTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[1]->tvec().device(*dev.edevice) = ts[1]->tvec() * (scale * gscale);
  ts[2]->tvec().device(*dev.edevice) = ts[2]->tvec() * rho + ts[1]->tvec().square() * (1.f - rho);
  ts[1]->tvec().device(*dev.edevice) = - ts[1]->tvec() * (ts[3]->tvec() + epsilon).sqrt() / (ts[2]->tvec() + epsilon).sqrt();
  ts[3]->tvec().device(*dev.edevice) = ts[3]->tvec() * rho + ts[1]->tvec().square() * (1.f - rho);
  ts[0]->tvec().device(*dev.edevice) += ts[1]->tvec() / global_weight_decay.CurrentWeightDecay();
}
CNN_TRAINER_INST_DEV_IMPL(AdadeltaTrainer)

#ifndef __CUDACC__
void AdadeltaTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &hg[idx].h, &hd[idx].h});
}
void AdadeltaTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &hlg[idx].h[lidx], &hld[idx].h[lidx]});
}
void AdadeltaTrainer::alloc_impl() {
  hg = AllocateShadowParameters(*model);
  hlg = AllocateShadowLookupParameters(*model);
  hd = AllocateShadowParameters(*model);
  hld = AllocateShadowLookupParameters(*model);
}
#endif

// --- RmsPropTrainer
// TODO: This is not finished yet, because it memorizes a scalar for each set of parameters, not each parameter itself.
//       We could implement this with one tensor for each scalar, but this is pretty wasteful

// Perform update of ts[0]=parameters, ts[1]=gradients
template <class MyDevice>
void RmsPropTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // real& d2 = hg[pi++];
  // real g2 = p->g.vec().squaredNorm();
  // d2 = rho * d2 + (1.f - rho) * g2;
  // p->values.vec() -= ((eta * scale * gscale / sqrt(d2 + epsilon)) * p->g.vec()) / global_weight_decay.CurrentWeightDecay();
}
CNN_TRAINER_INST_DEV_IMPL(RmsPropTrainer)

#ifndef __CUDACC__
void RmsPropTrainer::update_params(real scale, real gscale, size_t idx) {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // auto & p = model->parameters_list()[idx];
  // update_rule(scale, gscale, {&p->values, &p->g, &hg[idx].h, &hd[idx].h});
}
void RmsPropTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // auto & p = model->lookup_parameters_list()[idx];
  // update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &hlg[idx].h[lidx], &hld[idx].h[lidx]});
}
void RmsPropTrainer::alloc_impl() {
  throw std::runtime_error("RMSProp optimization not implemented yet.");
  // hg.resize(model->parameters_list().size());
  // unsigned pi = 0;
  // hlg.resize(model->lookup_parameters_list().size());
  // for (auto p : model->lookup_parameters_list()) {
  //   hlg[pi++].resize(p->size());
  // }
}
#endif

// --- AdamTrainer

// Perform update of ts[0]=parameters, ts[1]=gradients, ts[2]=mean, ts[3]=variance
template <class MyDevice>
void AdamTrainer::update_rule_dev(const MyDevice & dev, real scale, real gscale, const std::vector<Tensor*> & ts) {
  ts[1]->tvec().device(*dev.edevice) = ts[1]->tvec() * (scale * gscale);
  ts[2]->tvec().device(*dev.edevice) = ts[2]->tvec() * beta_1 + ts[1]->tvec() * (1.f - beta_1);
  ts[3]->tvec().device(*dev.edevice) = ts[3]->tvec() * beta_2 + ts[1]->tvec().square() * (1.f - beta_2);
  float s1 = 1 - pow(beta_1, updates+1);
  float s2 = 1 - pow(beta_2, updates+1);
  ts[0]->tvec().device(*dev.edevice) += ts[2]->tvec() / ((ts[3]->tvec() / s2).sqrt() + epsilon) * (-eta / s1 / global_weight_decay.CurrentWeightDecay());
}
CNN_TRAINER_INST_DEV_IMPL(AdamTrainer)

#ifndef __CUDACC__
void AdamTrainer::update_params(real scale, real gscale, size_t idx) {
  auto & p = model->parameters_list()[idx];
  update_rule(scale, gscale, {&p->values, &p->g, &m[idx].h, &v[idx].h});
}
void AdamTrainer::update_lookup_params(real scale, real gscale, size_t idx, size_t lidx) {
  auto & p = model->lookup_parameters_list()[idx];
  update_rule(scale, gscale, {&p->values[lidx], &p->grads[lidx], &lm[idx].h[lidx], &lv[idx].h[lidx]});
}
void AdamTrainer::alloc_impl() {
  m = AllocateShadowParameters(*model);
  lm = AllocateShadowLookupParameters(*model);
  v = AllocateShadowParameters(*model);
  lv = AllocateShadowLookupParameters(*model);
}
#endif

} // namespace cnn
<|endoftext|>


<commit_before>#include "ecs.hpp"
#include <algorithm>

namespace entity
{

	BaseComponent::Id BaseComponent::id_counter = 0;

	// Entity

	void Entity::kill()
	{
		entities->kill(*this);
	}

	bool Entity::is_alive() const
	{
		return entities->is_entity_alive(*this);
	}

	void Entity::tag(std::string tag_name)
	{
		entities->tag_entity(*this, tag_name);
	}

	void Entity::group(std::string group_name)
	{
		entities->group_entity(*this, group_name);
	}

	std::string Entity::to_string() const
	{
		std::string s = "entity_" + std::to_string(get_index()) + "_v" + std::to_string(get_version());
		return s;
	}

	// System

	void System::add_entity(Entity e)
	{
		entities.push_back(e);
	}

	void System::remove_entity(Entity e)
	{
		entities.erase(std::remove_if(entities.begin(), entities.end(),
			[&e](Entity other) { return e == other; }
		), entities.end());
	}

	// Entities

	Entities::Entities()
	{
	}

	void Entities::update()
	{
		for (auto e : created_entities) {
			update_systems(e);
		}
		created_entities.clear();

		for (auto e : killed_entities) {
			destroy_entity(e);
		}
		killed_entities.clear();
	}

	void Entities::update_systems(Entity e)
	{
		const auto &entity_component_mask = get_component_mask(e);

		for (auto &it : systems) {
			auto system = it.second;
			const auto &system_component_mask = system->get_component_mask();
			auto interest = (entity_component_mask & system_component_mask) == system_component_mask;

			if (interest) {
				system->add_entity(e);
			}
		}
	}

	Entity Entities::create()
	{
		auto e = create_entity();
		created_entities.push_back(e);
		return e;
	}

	void Entities::kill(Entity e)
	{
		killed_entities.push_back(e);
	}

	Entity Entities::create_entity()
	{
		Entity::Id index;

		if (free_ids.size() > MINIMUM_FREE_IDS) {
			index = free_ids.front();
			free_ids.pop_front();
		}
		else {
			versions.push_back(0);
			index = (unsigned int)versions.size() - 1;
			assert(index < (1 << Entity::INDEX_BITS));

			if (index >= component_masks.size()) {
				// TODO: grow by doubling?
				component_masks.resize(index + 1);
			}
		}

		assert(index < versions.size());
		Entity e(index, versions[index]);
		e.entities = this;

		return e;
	}

	void Entities::destroy_entity(Entity e)
	{
		const auto index = e.get_index();
		assert(index < versions.size());        // sanity check
		assert(index < component_masks.size());
		++versions[index];                      // increase the version for that id
		free_ids.push_back(index);              // make the id available for reuse
		component_masks[index].reset();         // reset the component mask for that id
	}

	bool Entities::is_entity_alive(Entity e) const
	{
		const auto index = e.get_index();
		assert(index < versions.size());
		return versions[index] == e.get_version();
	}

	const ComponentMask& Entities::get_component_mask(Entity e) const
	{
		const auto index = e.get_index();
		assert(index < component_masks.size());
		return component_masks[index];
	}

	void Entities::tag_entity(Entity e, std::string tag_name)
	{
		tagged_entities.emplace(tag_name, e);
	}

	bool Entities::has_tagged_entity(std::string tag_name) const
	{
		return tagged_entities.find(tag_name) != tagged_entities.end();
	}

	Entity Entities::get_entity_by_tag(std::string tag_name)
	{
		assert(has_tagged_entity(tag_name));
		return tagged_entities[tag_name];
	}

	void Entities::group_entity(Entity e, std::string group_name)
	{
		entity_groups.emplace(group_name, std::set<Entity>());
		entity_groups[group_name].emplace(e);
	}

	bool Entities::has_entity_group(std::string group_name) const
	{
		return entity_groups.find(group_name) != entity_groups.end();
	}

	std::vector<Entity> Entities::get_entity_group(std::string group_name)
	{
		assert(has_entity_group(group_name));
		auto &s = entity_groups[group_name];
		return std::vector<Entity>(s.begin(), s.end());
	}

}
<commit_msg>Allow is_alive() on invalid Entity.<commit_after>#include "ecs.hpp"
#include <algorithm>

namespace entity
{

	BaseComponent::Id BaseComponent::id_counter = 0;

	// Entity

	void Entity::kill()
	{
		entities->kill(*this);
	}

	bool Entity::is_alive() const
	{
		return entities && entities->is_entity_alive(*this);
	}

	void Entity::tag(std::string tag_name)
	{
		entities->tag_entity(*this, tag_name);
	}

	void Entity::group(std::string group_name)
	{
		entities->group_entity(*this, group_name);
	}

	std::string Entity::to_string() const
	{
		std::string s = "entity_" + std::to_string(get_index()) + "_v" + std::to_string(get_version());
		return s;
	}

	// System

	void System::add_entity(Entity e)
	{
		entities.push_back(e);
	}

	void System::remove_entity(Entity e)
	{
		entities.erase(std::remove_if(entities.begin(), entities.end(),
			[&e](Entity other) { return e == other; }
		), entities.end());
	}

	// Entities

	Entities::Entities()
	{
	}

	void Entities::update()
	{
		for (auto e : created_entities) {
			update_systems(e);
		}
		created_entities.clear();

		for (auto e : killed_entities) {
			destroy_entity(e);
		}
		killed_entities.clear();
	}

	void Entities::update_systems(Entity e)
	{
		const auto &entity_component_mask = get_component_mask(e);

		for (auto &it : systems) {
			auto system = it.second;
			const auto &system_component_mask = system->get_component_mask();
			auto interest = (entity_component_mask & system_component_mask) == system_component_mask;

			if (interest) {
				system->add_entity(e);
			}
		}
	}

	Entity Entities::create()
	{
		auto e = create_entity();
		created_entities.push_back(e);
		return e;
	}

	void Entities::kill(Entity e)
	{
		killed_entities.push_back(e);
	}

	Entity Entities::create_entity()
	{
		Entity::Id index;

		if (free_ids.size() > MINIMUM_FREE_IDS) {
			index = free_ids.front();
			free_ids.pop_front();
		}
		else {
			versions.push_back(0);
			index = (unsigned int)versions.size() - 1;
			assert(index < (1 << Entity::INDEX_BITS));

			if (index >= component_masks.size()) {
				// TODO: grow by doubling?
				component_masks.resize(index + 1);
			}
		}

		assert(index < versions.size());
		Entity e(index, versions[index]);
		e.entities = this;

		return e;
	}

	void Entities::destroy_entity(Entity e)
	{
		const auto index = e.get_index();
		assert(index < versions.size());        // sanity check
		assert(index < component_masks.size());
		++versions[index];                      // increase the version for that id
		free_ids.push_back(index);              // make the id available for reuse
		component_masks[index].reset();         // reset the component mask for that id
	}

	bool Entities::is_entity_alive(Entity e) const
	{
		const auto index = e.get_index();
		assert(index < versions.size());
		return versions[index] == e.get_version();
	}

	const ComponentMask& Entities::get_component_mask(Entity e) const
	{
		const auto index = e.get_index();
		assert(index < component_masks.size());
		return component_masks[index];
	}

	void Entities::tag_entity(Entity e, std::string tag_name)
	{
		tagged_entities.emplace(tag_name, e);
	}

	bool Entities::has_tagged_entity(std::string tag_name) const
	{
		return tagged_entities.find(tag_name) != tagged_entities.end();
	}

	Entity Entities::get_entity_by_tag(std::string tag_name)
	{
		assert(has_tagged_entity(tag_name));
		return tagged_entities[tag_name];
	}

	void Entities::group_entity(Entity e, std::string group_name)
	{
		entity_groups.emplace(group_name, std::set<Entity>());
		entity_groups[group_name].emplace(e);
	}

	bool Entities::has_entity_group(std::string group_name) const
	{
		return entity_groups.find(group_name) != entity_groups.end();
	}

	std::vector<Entity> Entities::get_entity_group(std::string group_name)
	{
		assert(has_entity_group(group_name));
		auto &s = entity_groups[group_name];
		return std::vector<Entity>(s.begin(), s.end());
	}

}
<|endoftext|>


<commit_before>#include "model.h"
#include <fstream>
#include <vector>
#include <cstring>
#include <sstream>
#include <unordered_map>
#include <functional>
#include <algorithm>
#include <limits>
#include "util.h"

using std::transform;
using std::ifstream;
using std::istringstream;
using std::vector;
using std::cerr;
using std::unordered_map;
using std::make_pair;
using std::pair;

namespace RawModelLoad
{
  struct tex_coord
  {
    float u, v;
  };

  struct tri_ref
  {
    int v[3];
  };

  struct vertex_ref
  {
    int p, t, n;
    bool operator==(const vertex_ref& b) const
    {
      return p == b.p && t == b.t && n == b.n;
    }
  };

}

RawModel::RawModel() : verts(), tris(), has_tex(false)
{
}

namespace std
{
  template <>
  struct hash<RawModelLoad::vertex_ref>
  {
  public:
    size_t operator ()(const RawModelLoad::vertex_ref& vr) const
    {
      size_t seed = 0;
      hash_combine(seed, vr.n);
      hash_combine(seed, vr.p);
      hash_combine(seed, vr.t);
      return seed;
    }
  };
}
using namespace RawModelLoad;

vertex_ref parse_obj_vertex_ref(string token);

RawModelLoadStatus RawModel::load_obj_model(string filename)
{
  RawModelLoadStatus mls;
  mls.success = false;

  ifstream fin(filename);
  if (!fin)
  {
    cerr << "Could not load model.\n";
    return mls;
  }

  vector<Vec3> vertex_list;
  vector<Vec3> normal_list;
  vector<tex_coord> tc_list;
  vector<vertex_ref> ref_list;
  vector<tri_ref> tri_list;

#define BUF_SIZE 1024
  char linebuf[BUF_SIZE];

  int line_count = 0;
  while (!fin.eof())
  {
    ++line_count;
    fin.getline(linebuf, BUF_SIZE);
    if (fin.fail())
    {
      if (fin.eof())
	break;

      cerr << "Line " << line_count << " in obj too long.";
      return mls;
    }
    string str(linebuf);

    if (str.empty() || str.find('#') != string::npos)
      continue;

    istringstream ss(str);

    string line_type;
    ss >> line_type;
    if (line_type == "v")
    {
      Vec3 v;
      ss >> v;
      vertex_list.push_back(v);
    }
    else if (line_type == "vn")
    {
      Vec3 v;
      ss >> v;
      normal_list.push_back(v);
    }
    else if (line_type == "vt")
    {
      tex_coord tc;
      ss >> tc.u >> tc.v;
      tc_list.push_back(tc);
    }
    else if (line_type == "f")
    {
      string part;
      int sidx = ref_list.size();
      int num_verts = 0;
      while (true)
      {
	ss >> part;
        if (!ss)
          break;
	ref_list.push_back(parse_obj_vertex_ref(part));
	++num_verts;
      }
      for (int i = 0; i < num_verts - 2; ++i)
      {
	tri_ref tri{sidx, sidx+i+1, sidx+i+2};
	tri_list.emplace_back(tri);
      }
    }
    else
    {
      cerr << "unknown line type: " << line_type << "\n";
      return mls;
    }
  }

  mls = load_from_parts(vertex_list, normal_list,
			tc_list, ref_list,
			tri_list);

  if (mls.success && !mls.has_normals)
  {
    compute_normals();
    mls.has_normals = true;
  }
  has_tex = mls.has_tex;
  return mls;
}

RawModelLoadStatus RawModel::load_stl_model(string filename)
{
  RawModelLoadStatus mls;
  mls.success = false;

  ifstream fin(filename);
  if (!fin)
  {
    cerr << "Could not open model file (" << filename << ").\n";
    return mls;
  }

  vector<Vec3> vertex_list;
  vector<Vec3> normal_list;
  vector<tex_coord> tc_list;
  vector<vertex_ref> ref_list;
  vector<tri_ref> tri_list;

#define BUF_SIZE 1024
  char linebuf[BUF_SIZE];

  int line_count = 0;
  while (!fin.eof())
  {
    ++line_count;
    fin.getline(linebuf, BUF_SIZE);
    if (fin.fail())
    {
      if (fin.eof())
	break;

      cerr << "Line " << line_count << " in obj too long.";
      return mls;
    }
    string str(linebuf);

    if (str.empty())
      continue;

    istringstream ss(str);

    string line_type;
    ss >> line_type;
    if (line_type == "vertex")
    {
      Vec3 v;
      ss >> v;
      vertex_list.push_back(v);
      ref_list.emplace_back(vertex_ref{(int)ref_list.size(), -1, -1});
    }
    else if (line_type == "endloop")
    {
      int num_tris = tri_list.size();
      tri_ref tri{num_tris*3, num_tris*3+1, num_tris*3+2};
      tri_list.emplace_back(tri);
    }
    else if (line_type == "facet" || line_type == "outer" ||
             line_type == "endfacet" || line_type == "solid")
    {
    }
    else if (line_type == "endsolid")
      break;
    else
    {
      cerr << "unknown line type: " << line_type << "\n";
      return mls;
    }
  }

  mls = load_from_parts(vertex_list, normal_list,
			tc_list, ref_list,
			tri_list);

  if (mls.success && !mls.has_normals)
  {
    compute_normals();
    mls.has_normals = true;
  }
  has_tex = mls.has_tex;
  return mls;
}

RawModelLoadStatus RawModel::load_from_parts(const vector<Vec3>& vertex_list, const vector<Vec3>& normal_list,
					     const vector<tex_coord>& tc_list, const vector<vertex_ref>& vertex_ref_list,
					     const vector<tri_ref>& tri_list)
{
  clear();

  unordered_map<vertex_ref, int> ref_map;

  RawModelLoadStatus mls;
  mls.success = true;
  mls.has_normals = normal_list.size() > 0;
  mls.has_tex = tc_list.size() > 0;

  for (const auto& tr: tri_list)
  {
    Triangle tri;
    for (int j = 0; j < 3; ++j)
    {
      int vidx = tr.v[j];
      auto vr = vertex_ref_list[vidx];

      // Add 'new' vertices.
      if (ref_map.find(vr) == ref_map.end())
      {
	ref_map.insert(make_pair(vr, ref_map.size()));
	Vertex v;

	v.position = vertex_list[vr.p];
	if (vr.t >= 0)
	{
	  v.u = tc_list[vr.t].u;
	  v.v = tc_list[vr.t].v;
	}
	if (vr.n >= 0)
	{
	  v.normal = normal_list[vr.n];
	}
	verts.push_back(v);
      }

      tri.v[j] = ref_map[vr];
    }
    tris.push_back(tri);
  }

  return mls;
}

void RawModel::clear()
{
  verts.clear();
  tris.clear();
  has_tex = false;
}

bounds::AABB RawModel::bounding_box() const
{
  Vec3 min{std::numeric_limits<scalar>::max()};
  Vec3 max{std::numeric_limits<scalar>::min()};
  min = accumulate(verts.begin(), verts.end(), min,
                   [] (const Vec3& v, const Vertex& w) { return ::min(v, w.position); });
  max = accumulate(verts.begin(), verts.end(), max,
                   [] (const Vec3& v, const Vertex& w) { return ::max(v, w.position); });

  return bounds::AABB{min, max};
}

bounds::Sphere RawModel::bounding_sphere() const
{
  Vec3 p1 = verts[0].position;

  Vec3 p2;
  scalar max_dist2 = 0;
  for (const auto& v: verts)
  {
    scalar dist2 = (v.position - p1).norm2();
    if (max_dist2 < dist2)
    {
      max_dist2 = dist2;
      p2 = v.position;
    }
  }

  Vec3 p3;
  max_dist2 = 0;
  for (const auto& v: verts)
  {
    scalar dist2 = (v.position - p2).norm2();
    if (max_dist2 < dist2)
    {
      max_dist2 = dist2;
      p3 = v.position;
    }
  }

  // Take the midpoint as the center, and find the best radius
  Vec3 mid = (p2 + p3) * 0.5;

  scalar radius = sqrt(accumulate(verts.begin(), verts.end(), (mid - p2).norm2(),
                                  /* This could be generic/polymorphic, but only
                                     in c++1y. */
                                  [&mid](scalar r2, const Vertex& v) {
                                    return max(r2, (mid - v.position).norm2());
                                  }));

  return bounds::Sphere(mid, radius);
}

void RawModel::compute_normals()
{
  vector<Vec3> normal_sum( verts.size() );
  vector<float> normal_weight( verts.size() );

  for (const auto& tri: tris)
  {
    // compute the normal, and compute the angle
    auto p1 = verts[tri.v[1]].position - verts[tri.v[0]].position;
    auto p2 = verts[tri.v[1]].position - verts[tri.v[0]].position;
    Vec3 face_normal = p1.cross(p2).normal();
    for (int i: tri.v)
      normal_sum[i] += face_normal;
  }
  for (size_t i = 0; i < verts.size(); ++i)
    verts[i].normal = normal_sum[i].normal();
}

void RawModel::rescale(const bounds::AABB& new_bb)
{
  auto bb = bounding_box();
  auto w = bb.max - bb.min;
  for (auto& f: w.v)
    if (f < EPSILON)
      f = 1;

  auto scale = (new_bb.max - new_bb.min).elem_div(w);

  for (auto& v: verts)
  {
    v.position = (v.position - bb.min).elem_mult(scale) + new_bb.min;
    v.normal = v.normal.elem_div(scale).normal();
  }
}

void RawModel::rescale(const bounds::Sphere& new_bs)
{
  const auto bs = bounding_sphere();

  auto scale = new_bs.radius / bs.radius;
  for (auto& v: verts)
  {
    v.position = (v.position - bs.center) * scale + new_bs.center;
  }
}

/*
 * A face in a .obj file is 'f' followed by a list of vertex references.
 *
 */
vertex_ref parse_obj_vertex_ref(string token) {

  vertex_ref ref{-1, -1, -1};
  auto pos = token.find('/');
  if (pos == string::npos)
  {
    ref.p = std::stoi(token) - 1;
    return ref;
  }
  ref.p = std::stoi(token.substr(pos)) - 1;

  auto tpos = token.find('/', pos+1);
  if (tpos == string::npos)
  {
    ref.t = std::stoi(token.substr(pos+1, tpos-pos-1)) - 1;
    return ref;
  }

  if (tpos > pos + 1)
  {
    ref.t = std::stoi(token.substr(pos+1, tpos-pos-1)) - 1;
  }
  ref.n = std::stoi(token.substr(tpos+1)) - 1;
  return ref;
}
<commit_msg>uses polymorhpic lambda in model.cpp<commit_after>#include "model.h"
#include <fstream>
#include <vector>
#include <cstring>
#include <sstream>
#include <unordered_map>
#include <functional>
#include <algorithm>
#include <limits>
#include "util.h"

using std::transform;
using std::ifstream;
using std::istringstream;
using std::vector;
using std::cerr;
using std::unordered_map;
using std::make_pair;
using std::pair;

namespace RawModelLoad
{
  struct tex_coord
  {
    float u, v;
  };

  struct tri_ref
  {
    int v[3];
  };

  struct vertex_ref
  {
    int p, t, n;
    bool operator==(const vertex_ref& b) const
    {
      return p == b.p && t == b.t && n == b.n;
    }
  };

}

RawModel::RawModel() : verts(), tris(), has_tex(false)
{
}

namespace std
{
  template <>
  struct hash<RawModelLoad::vertex_ref>
  {
  public:
    size_t operator ()(const RawModelLoad::vertex_ref& vr) const
    {
      size_t seed = 0;
      hash_combine(seed, vr.n);
      hash_combine(seed, vr.p);
      hash_combine(seed, vr.t);
      return seed;
    }
  };
}
using namespace RawModelLoad;

vertex_ref parse_obj_vertex_ref(string token);

RawModelLoadStatus RawModel::load_obj_model(string filename)
{
  RawModelLoadStatus mls;
  mls.success = false;

  ifstream fin(filename);
  if (!fin)
  {
    cerr << "Could not load model.\n";
    return mls;
  }

  vector<Vec3> vertex_list;
  vector<Vec3> normal_list;
  vector<tex_coord> tc_list;
  vector<vertex_ref> ref_list;
  vector<tri_ref> tri_list;

#define BUF_SIZE 1024
  char linebuf[BUF_SIZE];

  int line_count = 0;
  while (!fin.eof())
  {
    ++line_count;
    fin.getline(linebuf, BUF_SIZE);
    if (fin.fail())
    {
      if (fin.eof())
	break;

      cerr << "Line " << line_count << " in obj too long.";
      return mls;
    }
    string str(linebuf);

    if (str.empty() || str.find('#') != string::npos)
      continue;

    istringstream ss(str);

    string line_type;
    ss >> line_type;
    if (line_type == "v")
    {
      Vec3 v;
      ss >> v;
      vertex_list.push_back(v);
    }
    else if (line_type == "vn")
    {
      Vec3 v;
      ss >> v;
      normal_list.push_back(v);
    }
    else if (line_type == "vt")
    {
      tex_coord tc;
      ss >> tc.u >> tc.v;
      tc_list.push_back(tc);
    }
    else if (line_type == "f")
    {
      string part;
      int sidx = ref_list.size();
      int num_verts = 0;
      while (true)
      {
	ss >> part;
        if (!ss)
          break;
	ref_list.push_back(parse_obj_vertex_ref(part));
	++num_verts;
      }
      for (int i = 0; i < num_verts - 2; ++i)
      {
	tri_ref tri{sidx, sidx+i+1, sidx+i+2};
	tri_list.emplace_back(tri);
      }
    }
    else
    {
      cerr << "unknown line type: " << line_type << "\n";
      return mls;
    }
  }

  mls = load_from_parts(vertex_list, normal_list,
			tc_list, ref_list,
			tri_list);

  if (mls.success && !mls.has_normals)
  {
    compute_normals();
    mls.has_normals = true;
  }
  has_tex = mls.has_tex;
  return mls;
}

RawModelLoadStatus RawModel::load_stl_model(string filename)
{
  RawModelLoadStatus mls;
  mls.success = false;

  ifstream fin(filename);
  if (!fin)
  {
    cerr << "Could not open model file (" << filename << ").\n";
    return mls;
  }

  vector<Vec3> vertex_list;
  vector<Vec3> normal_list;
  vector<tex_coord> tc_list;
  vector<vertex_ref> ref_list;
  vector<tri_ref> tri_list;

#define BUF_SIZE 1024
  char linebuf[BUF_SIZE];

  int line_count = 0;
  while (!fin.eof())
  {
    ++line_count;
    fin.getline(linebuf, BUF_SIZE);
    if (fin.fail())
    {
      if (fin.eof())
	break;

      cerr << "Line " << line_count << " in obj too long.";
      return mls;
    }
    string str(linebuf);

    if (str.empty())
      continue;

    istringstream ss(str);

    string line_type;
    ss >> line_type;
    if (line_type == "vertex")
    {
      Vec3 v;
      ss >> v;
      vertex_list.push_back(v);
      ref_list.emplace_back(vertex_ref{(int)ref_list.size(), -1, -1});
    }
    else if (line_type == "endloop")
    {
      int num_tris = tri_list.size();
      tri_ref tri{num_tris*3, num_tris*3+1, num_tris*3+2};
      tri_list.emplace_back(tri);
    }
    else if (line_type == "facet" || line_type == "outer" ||
             line_type == "endfacet" || line_type == "solid")
    {
    }
    else if (line_type == "endsolid")
      break;
    else
    {
      cerr << "unknown line type: " << line_type << "\n";
      return mls;
    }
  }

  mls = load_from_parts(vertex_list, normal_list,
			tc_list, ref_list,
			tri_list);

  if (mls.success && !mls.has_normals)
  {
    compute_normals();
    mls.has_normals = true;
  }
  has_tex = mls.has_tex;
  return mls;
}

RawModelLoadStatus RawModel::load_from_parts(const vector<Vec3>& vertex_list, const vector<Vec3>& normal_list,
					     const vector<tex_coord>& tc_list, const vector<vertex_ref>& vertex_ref_list,
					     const vector<tri_ref>& tri_list)
{
  clear();

  unordered_map<vertex_ref, int> ref_map;

  RawModelLoadStatus mls;
  mls.success = true;
  mls.has_normals = normal_list.size() > 0;
  mls.has_tex = tc_list.size() > 0;

  for (const auto& tr: tri_list)
  {
    Triangle tri;
    for (int j = 0; j < 3; ++j)
    {
      int vidx = tr.v[j];
      auto vr = vertex_ref_list[vidx];

      // Add 'new' vertices.
      if (ref_map.find(vr) == ref_map.end())
      {
	ref_map.insert(make_pair(vr, ref_map.size()));
	Vertex v;

	v.position = vertex_list[vr.p];
	if (vr.t >= 0)
	{
	  v.u = tc_list[vr.t].u;
	  v.v = tc_list[vr.t].v;
	}
	if (vr.n >= 0)
	{
	  v.normal = normal_list[vr.n];
	}
	verts.push_back(v);
      }

      tri.v[j] = ref_map[vr];
    }
    tris.push_back(tri);
  }

  return mls;
}

void RawModel::clear()
{
  verts.clear();
  tris.clear();
  has_tex = false;
}

bounds::AABB RawModel::bounding_box() const
{
  Vec3 min{std::numeric_limits<scalar>::max()};
  Vec3 max{std::numeric_limits<scalar>::min()};
  min = accumulate(verts.begin(), verts.end(), min,
                   [] (const Vec3& v, const Vertex& w) { return ::min(v, w.position); });
  max = accumulate(verts.begin(), verts.end(), max,
                   [] (const Vec3& v, const Vertex& w) { return ::max(v, w.position); });

  return bounds::AABB{min, max};
}

bounds::Sphere RawModel::bounding_sphere() const
{
  Vec3 p1 = verts[0].position;

  Vec3 p2;
  scalar max_dist2 = 0;
  for (const auto& v: verts)
  {
    scalar dist2 = (v.position - p1).norm2();
    if (max_dist2 < dist2)
    {
      max_dist2 = dist2;
      p2 = v.position;
    }
  }

  Vec3 p3;
  max_dist2 = 0;
  for (const auto& v: verts)
  {
    scalar dist2 = (v.position - p2).norm2();
    if (max_dist2 < dist2)
    {
      max_dist2 = dist2;
      p3 = v.position;
    }
  }

  // Take the midpoint as the center, and find the best radius
  Vec3 mid = (p2 + p3) * 0.5;

  scalar radius = sqrt(accumulate(verts.begin(), verts.end(), (mid - p2).norm2(),
                                  /* This could be generic/polymorphic, but only
                                     in c++1y. */
//                                  [&mid](scalar r2, const Vertex& v) {
                                  [&mid](auto r2, auto&& v) {
                                    return max(r2, (mid - v.position).norm2());
                                  }));

  return bounds::Sphere(mid, radius);
}

void RawModel::compute_normals()
{
  vector<Vec3> normal_sum( verts.size() );
  vector<float> normal_weight( verts.size() );

  for (const auto& tri: tris)
  {
    // compute the normal, and compute the angle
    auto p1 = verts[tri.v[1]].position - verts[tri.v[0]].position;
    auto p2 = verts[tri.v[1]].position - verts[tri.v[0]].position;
    Vec3 face_normal = p1.cross(p2).normal();
    for (int i: tri.v)
      normal_sum[i] += face_normal;
  }
  for (size_t i = 0; i < verts.size(); ++i)
    verts[i].normal = normal_sum[i].normal();
}

void RawModel::rescale(const bounds::AABB& new_bb)
{
  auto bb = bounding_box();
  auto w = bb.max - bb.min;
  for (auto& f: w.v)
    if (f < EPSILON)
      f = 1;

  auto scale = (new_bb.max - new_bb.min).elem_div(w);

  for (auto& v: verts)
  {
    v.position = (v.position - bb.min).elem_mult(scale) + new_bb.min;
    v.normal = v.normal.elem_div(scale).normal();
  }
}

void RawModel::rescale(const bounds::Sphere& new_bs)
{
  const auto bs = bounding_sphere();

  auto scale = new_bs.radius / bs.radius;
  for (auto& v: verts)
  {
    v.position = (v.position - bs.center) * scale + new_bs.center;
  }
}

/*
 * A face in a .obj file is 'f' followed by a list of vertex references.
 *
 */
vertex_ref parse_obj_vertex_ref(string token) {

  vertex_ref ref{-1, -1, -1};
  auto pos = token.find('/');
  if (pos == string::npos)
  {
    ref.p = std::stoi(token) - 1;
    return ref;
  }
  ref.p = std::stoi(token.substr(pos)) - 1;

  auto tpos = token.find('/', pos+1);
  if (tpos == string::npos)
  {
    ref.t = std::stoi(token.substr(pos+1, tpos-pos-1)) - 1;
    return ref;
  }

  if (tpos > pos + 1)
  {
    ref.t = std::stoi(token.substr(pos+1, tpos-pos-1)) - 1;
  }
  ref.n = std::stoi(token.substr(tpos+1)) - 1;
  return ref;
}
<|endoftext|>