<commit_before>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
 * This file is part of the LibreOffice project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * This file incorporates work covered by the following license notice:
 *
 *   Licensed to the Apache Software Foundation (ASF) under one or more
 *   contributor license agreements. See the NOTICE file distributed
 *   with this work for additional information regarding copyright
 *   ownership. The ASF licenses this file to you under the Apache
 *   License, Version 2.0 (the "License"); you may not use this file
 *   except in compliance with the License. You may obtain a copy of
 *   the License at http://www.apache.org/licenses/LICENSE-2.0 .
 */

#include <stdio.h>

#include <tools/vcompat.hxx>

#include <vcl/graphictools.hxx>

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

SvtGraphicFill::Transform::Transform()
{
    matrix[0] = 1.0; matrix[1] = 0.0; matrix[2] = 0.0;
    matrix[3] = 0.0; matrix[4] = 1.0; matrix[5] = 0.0;
}

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

SvtGraphicStroke::SvtGraphicStroke() :
    maPath(),
    maStartArrow(),
    maEndArrow(),
    mfTransparency(),
    mfStrokeWidth(),
    maCapType(),
    maJoinType(),
    mfMiterLimit( 3.0 ),
    maDashArray()
{
}

SvtGraphicStroke::SvtGraphicStroke( const Polygon&      rPath,
                                    const PolyPolygon&  rStartArrow,
                                    const PolyPolygon&  rEndArrow,
                                    double              fTransparency,
                                    double              fStrokeWidth,
                                    CapType             aCap,
                                    JoinType            aJoin,
                                    double              fMiterLimit,
                                    const DashArray&    rDashArray  ) :
    maPath( rPath ),
    maStartArrow( rStartArrow ),
    maEndArrow( rEndArrow ),
    mfTransparency( fTransparency ),
    mfStrokeWidth( fStrokeWidth ),
    maCapType( aCap ),
    maJoinType( aJoin ),
    mfMiterLimit( fMiterLimit ),
    maDashArray( rDashArray )
{
}

void SvtGraphicStroke::getPath( Polygon& rPath ) const
{
    rPath = maPath;
}

void SvtGraphicStroke::getStartArrow( PolyPolygon& rPath ) const
{
    rPath = maStartArrow;
}

void SvtGraphicStroke::getEndArrow( PolyPolygon& rPath ) const
{
    rPath = maEndArrow;
}

double SvtGraphicStroke::getTransparency() const
{
    return mfTransparency;
}

double SvtGraphicStroke::getStrokeWidth() const
{
    return mfStrokeWidth;
}

SvtGraphicStroke::CapType SvtGraphicStroke::getCapType() const
{
    return maCapType;
}

SvtGraphicStroke::JoinType SvtGraphicStroke::getJoinType() const
{
    return maJoinType;
}

double SvtGraphicStroke::getMiterLimit() const
{
    return mfMiterLimit;
}

void SvtGraphicStroke::getDashArray( DashArray& rDashArray ) const
{
    rDashArray = maDashArray;
}

void SvtGraphicStroke::setPath( const Polygon& rPoly )
{
    maPath = rPoly;
}

void SvtGraphicStroke::scale( double fXScale, double fYScale )
{
    // Clearly scaling stroke-width for fat lines is rather a problem
    maPath.Scale( fXScale, fYScale );

    double fScale = sqrt (fabs (fXScale * fYScale) ); // clearly not ideal.
    fprintf( stderr, " HIT A SCALING ! by %g %g %g\n",
             (double) fXScale, (double) fYScale, (double) fScale );
    mfStrokeWidth *= fScale;
    mfMiterLimit *= fScale;

    maStartArrow.Scale( fXScale, fYScale );
    maEndArrow.Scale( fXScale, fYScale );
}

SvStream& operator<<( SvStream& rOStm, const SvtGraphicStroke& rClass )
{
    VersionCompat aCompat( rOStm, STREAM_WRITE, 1 );

    rClass.maPath.Write( rOStm );
    rClass.maStartArrow.Write( rOStm );
    rClass.maEndArrow.Write( rOStm );
    rOStm << rClass.mfTransparency;
    rOStm << rClass.mfStrokeWidth;
    sal_uInt16 nTmp = sal::static_int_cast<sal_uInt16>( rClass.maCapType );
    rOStm << nTmp;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maJoinType );
    rOStm << nTmp;
    rOStm << rClass.mfMiterLimit;

    rOStm << static_cast<sal_uInt32>(rClass.maDashArray.size());
    size_t i;
    for(i=0; i<rClass.maDashArray.size(); ++i)
        rOStm << rClass.maDashArray[i];

    return rOStm;
}

SvStream& operator>>( SvStream& rIStm, SvtGraphicStroke& rClass )
{
    VersionCompat aCompat( rIStm, STREAM_READ );

    rClass.maPath.Read( rIStm );
    rClass.maStartArrow.Read( rIStm );
    rClass.maEndArrow.Read( rIStm );
    rIStm >> rClass.mfTransparency;
    rIStm >> rClass.mfStrokeWidth;
    sal_uInt16 nTmp;
    rIStm >> nTmp;
    rClass.maCapType = SvtGraphicStroke::CapType(nTmp);
    rIStm >> nTmp;
    rClass.maJoinType = SvtGraphicStroke::JoinType(nTmp);
    rIStm >> rClass.mfMiterLimit;

    sal_uInt32 nSize;
    rIStm >> nSize;
    rClass.maDashArray.resize(nSize);
    size_t i;
    for(i=0; i<rClass.maDashArray.size(); ++i)
        rIStm >> rClass.maDashArray[i];

    return rIStm;
}

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

SvtGraphicFill::SvtGraphicFill() :
    maPath(),
    maFillColor( COL_BLACK ),
    mfTransparency(),
    maFillRule(),
    maFillType(),
    maFillTransform(),
    maHatchType(),
    maHatchColor( COL_BLACK ),
    maGradientType(),
    maGradient1stColor( COL_BLACK ),
    maGradient2ndColor( COL_BLACK ),
    maGradientStepCount( gradientStepsInfinite ),
    maFillGraphic()
{
}

SvtGraphicFill::SvtGraphicFill( const PolyPolygon&  rPath,
                                Color               aFillColor,
                                double              fTransparency,
                                FillRule            aFillRule,
                                FillType            aFillType,
                                const Transform&    aFillTransform,
                                bool                bTiling,
                                HatchType           aHatchType,
                                Color               aHatchColor,
                                GradientType        aGradientType,
                                Color               aGradient1stColor,
                                Color               aGradient2ndColor,
                                sal_Int32           aGradientStepCount,
                                const Graphic&      aFillGraphic ) :
    maPath( rPath ),
    maFillColor( aFillColor ),
    mfTransparency( fTransparency ),
    maFillRule( aFillRule ),
    maFillType( aFillType ),
    maFillTransform( aFillTransform ),
    mbTiling( bTiling ),
    maHatchType( aHatchType ),
    maHatchColor( aHatchColor ),
    maGradientType( aGradientType ),
    maGradient1stColor( aGradient1stColor ),
    maGradient2ndColor( aGradient2ndColor ),
    maGradientStepCount( aGradientStepCount ),
    maFillGraphic( aFillGraphic )
{
}

void SvtGraphicFill::getPath( PolyPolygon& rPath ) const
{
    rPath = maPath;
}

Color SvtGraphicFill::getFillColor() const
{
    return maFillColor;
}

double SvtGraphicFill::getTransparency() const
{
    return mfTransparency;
}

SvtGraphicFill::FillRule SvtGraphicFill::getFillRule() const
{
    return maFillRule;
}

SvtGraphicFill::FillType SvtGraphicFill::getFillType() const
{
    return maFillType;
}

void SvtGraphicFill::getTransform( Transform& rTrans ) const
{
    rTrans = maFillTransform;
}

bool SvtGraphicFill::IsTiling() const
{
    return mbTiling;
}

bool SvtGraphicFill::isTiling() const
{
    return mbTiling;
}

SvtGraphicFill::GradientType SvtGraphicFill::getGradientType() const
{
    return maGradientType;
}

void SvtGraphicFill::getGraphic( Graphic& rGraphic ) const
{
    rGraphic = maFillGraphic;
}

void SvtGraphicFill::setPath( const PolyPolygon& rPath )
{
    maPath = rPath;
}

SvStream& operator<<( SvStream& rOStm, const SvtGraphicFill& rClass )
{
    VersionCompat aCompat( rOStm, STREAM_WRITE, 1 );

    rClass.maPath.Write( rOStm );
    rOStm << rClass.maFillColor;
    rOStm << rClass.mfTransparency;
    sal_uInt16 nTmp = sal::static_int_cast<sal_uInt16>( rClass.maFillRule );
    rOStm << nTmp;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maFillType );
    rOStm << nTmp;
    int i;
    for(i=0; i<SvtGraphicFill::Transform::MatrixSize; ++i)
        rOStm << rClass.maFillTransform.matrix[i];
    nTmp = rClass.mbTiling;
    rOStm << nTmp;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maHatchType );
    rOStm << nTmp;
    rOStm << rClass.maHatchColor;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maGradientType );
    rOStm << nTmp;
    rOStm << rClass.maGradient1stColor;
    rOStm << rClass.maGradient2ndColor;
    rOStm << rClass.maGradientStepCount;
    rOStm << rClass.maFillGraphic;

    return rOStm;
}

SvStream& operator>>( SvStream& rIStm, SvtGraphicFill& rClass )
{
    VersionCompat aCompat( rIStm, STREAM_READ );

    rClass.maPath.Read( rIStm );
    rIStm >> rClass.maFillColor;
    rIStm >> rClass.mfTransparency;
    sal_uInt16 nTmp;
    rIStm >> nTmp;
    rClass.maFillRule = SvtGraphicFill::FillRule( nTmp );
    rIStm >> nTmp;
    rClass.maFillType = SvtGraphicFill::FillType( nTmp );
    int i;
    for(i=0; i<SvtGraphicFill::Transform::MatrixSize; ++i)
        rIStm >> rClass.maFillTransform.matrix[i];
    rIStm >> nTmp;
    rClass.mbTiling = nTmp;
    rIStm >> nTmp;
    rClass.maHatchType = SvtGraphicFill::HatchType( nTmp );
    rIStm >> rClass.maHatchColor;
    rIStm >> nTmp;
    rClass.maGradientType = SvtGraphicFill::GradientType( nTmp );
    rIStm >> rClass.maGradient1stColor;
    rIStm >> rClass.maGradient2ndColor;
    rIStm >> rClass.maGradientStepCount;
    rIStm >> rClass.maFillGraphic;

    return rIStm;
}

/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
<commit_msg>remove an fprintf I left in in error.<commit_after>/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
 * This file is part of the LibreOffice project.
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 *
 * This file incorporates work covered by the following license notice:
 *
 *   Licensed to the Apache Software Foundation (ASF) under one or more
 *   contributor license agreements. See the NOTICE file distributed
 *   with this work for additional information regarding copyright
 *   ownership. The ASF licenses this file to you under the Apache
 *   License, Version 2.0 (the "License"); you may not use this file
 *   except in compliance with the License. You may obtain a copy of
 *   the License at http://www.apache.org/licenses/LICENSE-2.0 .
 */

#include <stdio.h>

#include <tools/vcompat.hxx>

#include <vcl/graphictools.hxx>

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

SvtGraphicFill::Transform::Transform()
{
    matrix[0] = 1.0; matrix[1] = 0.0; matrix[2] = 0.0;
    matrix[3] = 0.0; matrix[4] = 1.0; matrix[5] = 0.0;
}

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

SvtGraphicStroke::SvtGraphicStroke() :
    maPath(),
    maStartArrow(),
    maEndArrow(),
    mfTransparency(),
    mfStrokeWidth(),
    maCapType(),
    maJoinType(),
    mfMiterLimit( 3.0 ),
    maDashArray()
{
}

SvtGraphicStroke::SvtGraphicStroke( const Polygon&      rPath,
                                    const PolyPolygon&  rStartArrow,
                                    const PolyPolygon&  rEndArrow,
                                    double              fTransparency,
                                    double              fStrokeWidth,
                                    CapType             aCap,
                                    JoinType            aJoin,
                                    double              fMiterLimit,
                                    const DashArray&    rDashArray  ) :
    maPath( rPath ),
    maStartArrow( rStartArrow ),
    maEndArrow( rEndArrow ),
    mfTransparency( fTransparency ),
    mfStrokeWidth( fStrokeWidth ),
    maCapType( aCap ),
    maJoinType( aJoin ),
    mfMiterLimit( fMiterLimit ),
    maDashArray( rDashArray )
{
}

void SvtGraphicStroke::getPath( Polygon& rPath ) const
{
    rPath = maPath;
}

void SvtGraphicStroke::getStartArrow( PolyPolygon& rPath ) const
{
    rPath = maStartArrow;
}

void SvtGraphicStroke::getEndArrow( PolyPolygon& rPath ) const
{
    rPath = maEndArrow;
}

double SvtGraphicStroke::getTransparency() const
{
    return mfTransparency;
}

double SvtGraphicStroke::getStrokeWidth() const
{
    return mfStrokeWidth;
}

SvtGraphicStroke::CapType SvtGraphicStroke::getCapType() const
{
    return maCapType;
}

SvtGraphicStroke::JoinType SvtGraphicStroke::getJoinType() const
{
    return maJoinType;
}

double SvtGraphicStroke::getMiterLimit() const
{
    return mfMiterLimit;
}

void SvtGraphicStroke::getDashArray( DashArray& rDashArray ) const
{
    rDashArray = maDashArray;
}

void SvtGraphicStroke::setPath( const Polygon& rPoly )
{
    maPath = rPoly;
}

void SvtGraphicStroke::scale( double fXScale, double fYScale )
{
    // Clearly scaling stroke-width for fat lines is rather a problem
    maPath.Scale( fXScale, fYScale );

    double fScale = sqrt (fabs (fXScale * fYScale) ); // clearly not ideal.
    mfStrokeWidth *= fScale;
    mfMiterLimit *= fScale;

    maStartArrow.Scale( fXScale, fYScale );
    maEndArrow.Scale( fXScale, fYScale );
}

SvStream& operator<<( SvStream& rOStm, const SvtGraphicStroke& rClass )
{
    VersionCompat aCompat( rOStm, STREAM_WRITE, 1 );

    rClass.maPath.Write( rOStm );
    rClass.maStartArrow.Write( rOStm );
    rClass.maEndArrow.Write( rOStm );
    rOStm << rClass.mfTransparency;
    rOStm << rClass.mfStrokeWidth;
    sal_uInt16 nTmp = sal::static_int_cast<sal_uInt16>( rClass.maCapType );
    rOStm << nTmp;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maJoinType );
    rOStm << nTmp;
    rOStm << rClass.mfMiterLimit;

    rOStm << static_cast<sal_uInt32>(rClass.maDashArray.size());
    size_t i;
    for(i=0; i<rClass.maDashArray.size(); ++i)
        rOStm << rClass.maDashArray[i];

    return rOStm;
}

SvStream& operator>>( SvStream& rIStm, SvtGraphicStroke& rClass )
{
    VersionCompat aCompat( rIStm, STREAM_READ );

    rClass.maPath.Read( rIStm );
    rClass.maStartArrow.Read( rIStm );
    rClass.maEndArrow.Read( rIStm );
    rIStm >> rClass.mfTransparency;
    rIStm >> rClass.mfStrokeWidth;
    sal_uInt16 nTmp;
    rIStm >> nTmp;
    rClass.maCapType = SvtGraphicStroke::CapType(nTmp);
    rIStm >> nTmp;
    rClass.maJoinType = SvtGraphicStroke::JoinType(nTmp);
    rIStm >> rClass.mfMiterLimit;

    sal_uInt32 nSize;
    rIStm >> nSize;
    rClass.maDashArray.resize(nSize);
    size_t i;
    for(i=0; i<rClass.maDashArray.size(); ++i)
        rIStm >> rClass.maDashArray[i];

    return rIStm;
}

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

SvtGraphicFill::SvtGraphicFill() :
    maPath(),
    maFillColor( COL_BLACK ),
    mfTransparency(),
    maFillRule(),
    maFillType(),
    maFillTransform(),
    maHatchType(),
    maHatchColor( COL_BLACK ),
    maGradientType(),
    maGradient1stColor( COL_BLACK ),
    maGradient2ndColor( COL_BLACK ),
    maGradientStepCount( gradientStepsInfinite ),
    maFillGraphic()
{
}

SvtGraphicFill::SvtGraphicFill( const PolyPolygon&  rPath,
                                Color               aFillColor,
                                double              fTransparency,
                                FillRule            aFillRule,
                                FillType            aFillType,
                                const Transform&    aFillTransform,
                                bool                bTiling,
                                HatchType           aHatchType,
                                Color               aHatchColor,
                                GradientType        aGradientType,
                                Color               aGradient1stColor,
                                Color               aGradient2ndColor,
                                sal_Int32           aGradientStepCount,
                                const Graphic&      aFillGraphic ) :
    maPath( rPath ),
    maFillColor( aFillColor ),
    mfTransparency( fTransparency ),
    maFillRule( aFillRule ),
    maFillType( aFillType ),
    maFillTransform( aFillTransform ),
    mbTiling( bTiling ),
    maHatchType( aHatchType ),
    maHatchColor( aHatchColor ),
    maGradientType( aGradientType ),
    maGradient1stColor( aGradient1stColor ),
    maGradient2ndColor( aGradient2ndColor ),
    maGradientStepCount( aGradientStepCount ),
    maFillGraphic( aFillGraphic )
{
}

void SvtGraphicFill::getPath( PolyPolygon& rPath ) const
{
    rPath = maPath;
}

Color SvtGraphicFill::getFillColor() const
{
    return maFillColor;
}

double SvtGraphicFill::getTransparency() const
{
    return mfTransparency;
}

SvtGraphicFill::FillRule SvtGraphicFill::getFillRule() const
{
    return maFillRule;
}

SvtGraphicFill::FillType SvtGraphicFill::getFillType() const
{
    return maFillType;
}

void SvtGraphicFill::getTransform( Transform& rTrans ) const
{
    rTrans = maFillTransform;
}

bool SvtGraphicFill::IsTiling() const
{
    return mbTiling;
}

bool SvtGraphicFill::isTiling() const
{
    return mbTiling;
}

SvtGraphicFill::GradientType SvtGraphicFill::getGradientType() const
{
    return maGradientType;
}

void SvtGraphicFill::getGraphic( Graphic& rGraphic ) const
{
    rGraphic = maFillGraphic;
}

void SvtGraphicFill::setPath( const PolyPolygon& rPath )
{
    maPath = rPath;
}

SvStream& operator<<( SvStream& rOStm, const SvtGraphicFill& rClass )
{
    VersionCompat aCompat( rOStm, STREAM_WRITE, 1 );

    rClass.maPath.Write( rOStm );
    rOStm << rClass.maFillColor;
    rOStm << rClass.mfTransparency;
    sal_uInt16 nTmp = sal::static_int_cast<sal_uInt16>( rClass.maFillRule );
    rOStm << nTmp;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maFillType );
    rOStm << nTmp;
    int i;
    for(i=0; i<SvtGraphicFill::Transform::MatrixSize; ++i)
        rOStm << rClass.maFillTransform.matrix[i];
    nTmp = rClass.mbTiling;
    rOStm << nTmp;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maHatchType );
    rOStm << nTmp;
    rOStm << rClass.maHatchColor;
    nTmp = sal::static_int_cast<sal_uInt16>( rClass.maGradientType );
    rOStm << nTmp;
    rOStm << rClass.maGradient1stColor;
    rOStm << rClass.maGradient2ndColor;
    rOStm << rClass.maGradientStepCount;
    rOStm << rClass.maFillGraphic;

    return rOStm;
}

SvStream& operator>>( SvStream& rIStm, SvtGraphicFill& rClass )
{
    VersionCompat aCompat( rIStm, STREAM_READ );

    rClass.maPath.Read( rIStm );
    rIStm >> rClass.maFillColor;
    rIStm >> rClass.mfTransparency;
    sal_uInt16 nTmp;
    rIStm >> nTmp;
    rClass.maFillRule = SvtGraphicFill::FillRule( nTmp );
    rIStm >> nTmp;
    rClass.maFillType = SvtGraphicFill::FillType( nTmp );
    int i;
    for(i=0; i<SvtGraphicFill::Transform::MatrixSize; ++i)
        rIStm >> rClass.maFillTransform.matrix[i];
    rIStm >> nTmp;
    rClass.mbTiling = nTmp;
    rIStm >> nTmp;
    rClass.maHatchType = SvtGraphicFill::HatchType( nTmp );
    rIStm >> rClass.maHatchColor;
    rIStm >> nTmp;
    rClass.maGradientType = SvtGraphicFill::GradientType( nTmp );
    rIStm >> rClass.maGradient1stColor;
    rIStm >> rClass.maGradient2ndColor;
    rIStm >> rClass.maGradientStepCount;
    rIStm >> rClass.maFillGraphic;

    return rIStm;
}

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


<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Dave Coleman */

#include <moveit/rviz_plugin_render_tools/trajectory_visualization.h>

#include <moveit/rviz_plugin_render_tools/planning_link_updater.h>
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
#include <rviz/robot/robot.h>

#include <rviz/properties/property.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/bool_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/editable_enum_property.h>
#include <rviz/properties/color_property.h>
#include <moveit/trajectory_processing/trajectory_tools.h>

namespace moveit_rviz_plugin
{

TrajectoryVisualization::TrajectoryVisualization(rviz::Property *widget, rviz::Display *display)
  : display_(display)
  , widget_(widget)
  , animating_path_(false)
{
  trajectory_topic_property_ =
    new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path",
                               ros::message_traits::datatype<moveit_msgs::DisplayTrajectory>(),
                               "The topic on which the moveit_msgs::DisplayTrajectory messages are received",
                               widget,
                               SLOT(changedTrajectoryTopic()), this);

  display_path_visual_enabled_property_ =
    new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for visualisation purposes should be displayed",
                           widget,
                           SLOT(changedDisplayPathVisualEnabled()), this);

  display_path_collision_enabled_property_ =
    new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined for collision detection purposes should be displayed",
                           widget,
                           SLOT(changedDisplayPathCollisionEnabled()), this);

  robot_path_alpha_property_ =
    new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links",
                            widget,
                            SLOT(changedRobotPathAlpha()), this);
  robot_path_alpha_property_->setMin(0.0);
  robot_path_alpha_property_->setMax(1.0);

  state_display_time_property_ =  new rviz::EditableEnumProperty("State Display Time", "0.05 s",
                                                                 "The amount of wall-time to wait in between displaying states along a received trajectory path",
                                                                 widget,
                                                                 SLOT(changedStateDisplayTime()), this);
  state_display_time_property_->addOptionStd("REALTIME");
  state_display_time_property_->addOptionStd("0.05 s");
  state_display_time_property_->addOptionStd("0.1 s");
  state_display_time_property_->addOptionStd("0.5 s");

  loop_display_property_ =
    new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path is to be animated in a loop",
                           widget,
                           SLOT(changedLoopDisplay()), this);

  trail_display_property_ =
    new rviz::BoolProperty("Show Trail", false, "Show a path trail",
                           widget,
                           SLOT(changedShowTrail()), this);

  connect(this, SIGNAL(timeToShowNewTrail()), this, SLOT(changedShowTrail()));
}

TrajectoryVisualization::~TrajectoryVisualization()
{
  clearTrajectoryTrail();
  trajectory_message_to_display_.reset();
  displaying_trajectory_message_.reset();

  display_path_robot_.reset();
}

void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, ros::NodeHandle update_nh)
{
  // Save pointers for later use
  scene_node_ = scene_node;
  context_ = context;
  update_nh_ = update_nh;

  // Load trajectory robot
  display_path_robot_.reset(new RobotStateVisualization(scene_node_, context_, "Planned Path", widget_));
  display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
  display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  display_path_robot_->setVisible(false);
}

void TrajectoryVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model)
{
  robot_model_ = robot_model;

  // Error check
  if (!robot_model_)
  {
    ROS_ERROR_STREAM_NAMED("trajectory_visualization","No robot model found");
    return;
  }

  // Load robot state
  robot_state_.reset(new robot_state::RobotState(robot_model_));
  robot_state_->setToDefaultValues();

  // Load rviz robot
  display_path_robot_->load(*robot_model_->getURDF());
}

void TrajectoryVisualization::reset()
{
  clearTrajectoryTrail();
  trajectory_message_to_display_.reset();
  displaying_trajectory_message_.reset();
  animating_path_ = false;

  display_path_robot_->clear();
  display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
  display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  display_path_robot_->setVisible(false);

  if (!robot_model_)
    ROS_WARN_STREAM_NAMED("trajectory_visualization","No robot model found");
  else
    display_path_robot_->load(*robot_model_->getURDF());
}

void TrajectoryVisualization::clearTrajectoryTrail()
{
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
    delete trajectory_trail_[i];
  trajectory_trail_.clear();
}

void TrajectoryVisualization::changedLoopDisplay()
{
  display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
}

void TrajectoryVisualization::changedShowTrail()
{
  clearTrajectoryTrail();

  if (!trail_display_property_->getBool())
    return;
  robot_trajectory::RobotTrajectoryPtr t = trajectory_message_to_display_;
  if (!t)
    t = displaying_trajectory_message_;
  if (!t)
    return;

  trajectory_trail_.resize(t->getWayPointCount());
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
  {
    rviz::Robot *r = new rviz::Robot(scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), NULL);
    r->load(*robot_model_->getURDF());
    r->setVisualVisible(display_path_visual_enabled_property_->getBool());
    r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
    r->update(PlanningLinkUpdater(t->getWayPointPtr(i)));
    r->setVisible(display_->isEnabled() && (!animating_path_ || i <= current_state_));
    trajectory_trail_[i] = r;
  }
}

void TrajectoryVisualization::changedRobotPathAlpha()
{
  display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat());
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
    trajectory_trail_[i]->setAlpha(robot_path_alpha_property_->getFloat());
}

void TrajectoryVisualization::changedTrajectoryTopic()
{
  trajectory_topic_sub_.shutdown();
  if (!trajectory_topic_property_->getStdString().empty())
  {
    trajectory_topic_sub_ = update_nh_.subscribe(trajectory_topic_property_->getStdString(), 2, &TrajectoryVisualization::incomingDisplayTrajectory, this);
  }
}

void TrajectoryVisualization::changedDisplayPathVisualEnabled()
{
  if (display_->isEnabled())
  {
    display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
    display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
    for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
      trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool());
  }
}

void TrajectoryVisualization::changedStateDisplayTime()
{
}

void TrajectoryVisualization::changedDisplayPathCollisionEnabled()
{
  if (display_->isEnabled())
  {
    display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
    display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
    for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
      trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  }
}

void TrajectoryVisualization::onEnable()
{
  changedRobotPathAlpha(); // set alpha property

  display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
  display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  display_path_robot_->setVisible(displaying_trajectory_message_ && animating_path_);
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
  {
    trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool());
    trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool());
    trajectory_trail_[i]->setVisible(true);
  }

  changedTrajectoryTopic(); // load topic at startup if default used
}

void TrajectoryVisualization::onDisable()
{
  display_path_robot_->setVisible(false);
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
    trajectory_trail_[i]->setVisible(false);
  displaying_trajectory_message_.reset();

}

float TrajectoryVisualization::getStateDisplayTime()
{
  std::string tm = state_display_time_property_->getStdString();
  if (tm == "REALTIME")
    return -1.0;
  else
  {
    boost::replace_all(tm, "s", "");
    boost::trim(tm);
    float t = 0.05f;
    try
    {
      t = boost::lexical_cast<float>(tm);
    }
    catch(const boost::bad_lexical_cast &ex)
    {
      state_display_time_property_->setStdString("0.05 s");
    }
    return t;
  }
}

void TrajectoryVisualization::update(float wall_dt, float ros_dt)
{
  // Check if starting new trajectory
  if (!animating_path_ && !trajectory_message_to_display_ && loop_display_property_->getBool() && displaying_trajectory_message_)
  {
    animating_path_ = true;
    current_state_ = -1;
    current_state_time_ = std::numeric_limits<float>::infinity();
    display_path_robot_->setVisible(display_->isEnabled());
  }

  if (!animating_path_ && trajectory_message_to_display_ && !trajectory_message_to_display_->empty())
  {
    displaying_trajectory_message_ = trajectory_message_to_display_;
    display_path_robot_->setVisible(display_->isEnabled());
    trajectory_message_to_display_.reset();
    animating_path_ = true;
    current_state_ = -1;
    current_state_time_ = std::numeric_limits<float>::infinity();
    display_path_robot_->update(displaying_trajectory_message_->getFirstWayPointPtr());
  }

  if (animating_path_)
  {
    float tm = getStateDisplayTime();
    if (tm < 0.0) // if we should use realtime
      tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1);
    if (current_state_time_ > tm)
    {
      ++current_state_;
      if ((std::size_t) current_state_ < displaying_trajectory_message_->getWayPointCount())
      {
        display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(current_state_));
        for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
          trajectory_trail_[i]->setVisible(i <= current_state_);
      }
      else
      {
        animating_path_ = false;
        display_path_robot_->setVisible(loop_display_property_->getBool());
      }
      current_state_time_ = 0.0f;
    }
    current_state_time_ += wall_dt;
  }
}

void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
{
  // Error check
  if (!robot_state_ || !robot_model_)
  {
    ROS_ERROR_STREAM_NAMED("trajectory_visualization","No robot state or robot model loaded");
    return;
  }

  if (!msg->model_id.empty() && msg->model_id != robot_model_->getName())
    ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected",
             msg->model_id.c_str(), robot_model_->getName().c_str());

  trajectory_message_to_display_.reset();

  robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
  for (std::size_t i = 0 ; i < msg->trajectory.size() ; ++i)
  {
    if (t->empty())
    {
      t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]);
    }
    else
    {
      robot_trajectory::RobotTrajectory tmp(robot_model_, "");
      tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]);
      t->append(tmp, 0.0);
    }
  }

  if (!t->empty())
  {
    trajectory_message_to_display_.swap(t);
  }
  if (trail_display_property_->getBool())
  {
    // incomingDisplayTrajectory() can be called from a non-GUI thread, so here we
    // use a signal/slot connection to invoke changedShowTrail() in the GUI thread.
    Q_EMIT timeToShowNewTrail();
  }
}

} // namespace moveit_rviz_plugin
<commit_msg>fix segfault when disabling and re-enabling TrajectoryVisualization<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Dave Coleman */

#include <moveit/rviz_plugin_render_tools/trajectory_visualization.h>

#include <moveit/rviz_plugin_render_tools/planning_link_updater.h>
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
#include <rviz/robot/robot.h>

#include <rviz/properties/property.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/bool_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/editable_enum_property.h>
#include <rviz/properties/color_property.h>
#include <moveit/trajectory_processing/trajectory_tools.h>

namespace moveit_rviz_plugin
{

TrajectoryVisualization::TrajectoryVisualization(rviz::Property *widget, rviz::Display *display)
  : display_(display)
  , widget_(widget)
  , animating_path_(false)
{
  trajectory_topic_property_ =
    new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path",
                               ros::message_traits::datatype<moveit_msgs::DisplayTrajectory>(),
                               "The topic on which the moveit_msgs::DisplayTrajectory messages are received",
                               widget,
                               SLOT(changedTrajectoryTopic()), this);

  display_path_visual_enabled_property_ =
    new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for visualisation purposes should be displayed",
                           widget,
                           SLOT(changedDisplayPathVisualEnabled()), this);

  display_path_collision_enabled_property_ =
    new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined for collision detection purposes should be displayed",
                           widget,
                           SLOT(changedDisplayPathCollisionEnabled()), this);

  robot_path_alpha_property_ =
    new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links",
                            widget,
                            SLOT(changedRobotPathAlpha()), this);
  robot_path_alpha_property_->setMin(0.0);
  robot_path_alpha_property_->setMax(1.0);

  state_display_time_property_ =  new rviz::EditableEnumProperty("State Display Time", "0.05 s",
                                                                 "The amount of wall-time to wait in between displaying states along a received trajectory path",
                                                                 widget,
                                                                 SLOT(changedStateDisplayTime()), this);
  state_display_time_property_->addOptionStd("REALTIME");
  state_display_time_property_->addOptionStd("0.05 s");
  state_display_time_property_->addOptionStd("0.1 s");
  state_display_time_property_->addOptionStd("0.5 s");

  loop_display_property_ =
    new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path is to be animated in a loop",
                           widget,
                           SLOT(changedLoopDisplay()), this);

  trail_display_property_ =
    new rviz::BoolProperty("Show Trail", false, "Show a path trail",
                           widget,
                           SLOT(changedShowTrail()), this);

  connect(this, SIGNAL(timeToShowNewTrail()), this, SLOT(changedShowTrail()));
}

TrajectoryVisualization::~TrajectoryVisualization()
{
  clearTrajectoryTrail();
  trajectory_message_to_display_.reset();
  displaying_trajectory_message_.reset();

  display_path_robot_.reset();
}

void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, ros::NodeHandle update_nh)
{
  // Save pointers for later use
  scene_node_ = scene_node;
  context_ = context;
  update_nh_ = update_nh;

  // Load trajectory robot
  display_path_robot_.reset(new RobotStateVisualization(scene_node_, context_, "Planned Path", widget_));
  display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
  display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  display_path_robot_->setVisible(false);
}

void TrajectoryVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model)
{
  robot_model_ = robot_model;

  // Error check
  if (!robot_model_)
  {
    ROS_ERROR_STREAM_NAMED("trajectory_visualization","No robot model found");
    return;
  }

  // Load robot state
  robot_state_.reset(new robot_state::RobotState(robot_model_));
  robot_state_->setToDefaultValues();

  // Load rviz robot
  display_path_robot_->load(*robot_model_->getURDF());
}

void TrajectoryVisualization::reset()
{
  clearTrajectoryTrail();
  trajectory_message_to_display_.reset();
  displaying_trajectory_message_.reset();
  animating_path_ = false;

  display_path_robot_->clear();
  display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
  display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  display_path_robot_->setVisible(false);

  if (!robot_model_)
    ROS_WARN_STREAM_NAMED("trajectory_visualization","No robot model found");
  else
    display_path_robot_->load(*robot_model_->getURDF());
}

void TrajectoryVisualization::clearTrajectoryTrail()
{
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
    delete trajectory_trail_[i];
  trajectory_trail_.clear();
}

void TrajectoryVisualization::changedLoopDisplay()
{
  display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
}

void TrajectoryVisualization::changedShowTrail()
{
  clearTrajectoryTrail();

  if (!trail_display_property_->getBool())
    return;
  robot_trajectory::RobotTrajectoryPtr t = trajectory_message_to_display_;
  if (!t)
    t = displaying_trajectory_message_;
  if (!t)
    return;

  trajectory_trail_.resize(t->getWayPointCount());
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
  {
    rviz::Robot *r = new rviz::Robot(scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), NULL);
    r->load(*robot_model_->getURDF());
    r->setVisualVisible(display_path_visual_enabled_property_->getBool());
    r->setCollisionVisible(display_path_collision_enabled_property_->getBool());
    r->update(PlanningLinkUpdater(t->getWayPointPtr(i)));
    r->setVisible(display_->isEnabled() && (!animating_path_ || i <= current_state_));
    trajectory_trail_[i] = r;
  }
}

void TrajectoryVisualization::changedRobotPathAlpha()
{
  display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat());
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
    trajectory_trail_[i]->setAlpha(robot_path_alpha_property_->getFloat());
}

void TrajectoryVisualization::changedTrajectoryTopic()
{
  trajectory_topic_sub_.shutdown();
  if (!trajectory_topic_property_->getStdString().empty())
  {
    trajectory_topic_sub_ = update_nh_.subscribe(trajectory_topic_property_->getStdString(), 2, &TrajectoryVisualization::incomingDisplayTrajectory, this);
  }
}

void TrajectoryVisualization::changedDisplayPathVisualEnabled()
{
  if (display_->isEnabled())
  {
    display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
    display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
    for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
      trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool());
  }
}

void TrajectoryVisualization::changedStateDisplayTime()
{
}

void TrajectoryVisualization::changedDisplayPathCollisionEnabled()
{
  if (display_->isEnabled())
  {
    display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
    display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
    for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
      trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  }
}

void TrajectoryVisualization::onEnable()
{
  changedRobotPathAlpha(); // set alpha property

  display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
  display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
  display_path_robot_->setVisible(displaying_trajectory_message_ && animating_path_);
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
  {
    trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool());
    trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool());
    trajectory_trail_[i]->setVisible(true);
  }

  changedTrajectoryTopic(); // load topic at startup if default used
}

void TrajectoryVisualization::onDisable()
{
  display_path_robot_->setVisible(false);
  for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
    trajectory_trail_[i]->setVisible(false);
  displaying_trajectory_message_.reset();
  animating_path_ = false;
}

float TrajectoryVisualization::getStateDisplayTime()
{
  std::string tm = state_display_time_property_->getStdString();
  if (tm == "REALTIME")
    return -1.0;
  else
  {
    boost::replace_all(tm, "s", "");
    boost::trim(tm);
    float t = 0.05f;
    try
    {
      t = boost::lexical_cast<float>(tm);
    }
    catch(const boost::bad_lexical_cast &ex)
    {
      state_display_time_property_->setStdString("0.05 s");
    }
    return t;
  }
}

void TrajectoryVisualization::update(float wall_dt, float ros_dt)
{
  // Check if starting new trajectory
  if (!animating_path_ && !trajectory_message_to_display_ && loop_display_property_->getBool() && displaying_trajectory_message_)
  {
    animating_path_ = true;
    current_state_ = -1;
    current_state_time_ = std::numeric_limits<float>::infinity();
    display_path_robot_->setVisible(display_->isEnabled());
  }

  if (!animating_path_ && trajectory_message_to_display_ && !trajectory_message_to_display_->empty())
  {
    displaying_trajectory_message_ = trajectory_message_to_display_;
    display_path_robot_->setVisible(display_->isEnabled());
    trajectory_message_to_display_.reset();
    animating_path_ = true;
    current_state_ = -1;
    current_state_time_ = std::numeric_limits<float>::infinity();
    display_path_robot_->update(displaying_trajectory_message_->getFirstWayPointPtr());
  }

  if (animating_path_)
  {
    float tm = getStateDisplayTime();
    if (tm < 0.0) // if we should use realtime
      tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1);
    if (current_state_time_ > tm)
    {
      ++current_state_;
      if ((std::size_t) current_state_ < displaying_trajectory_message_->getWayPointCount())
      {
        display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(current_state_));
        for (std::size_t i = 0 ; i < trajectory_trail_.size() ; ++i)
          trajectory_trail_[i]->setVisible(i <= current_state_);
      }
      else
      {
        animating_path_ = false;
        display_path_robot_->setVisible(loop_display_property_->getBool());
      }
      current_state_time_ = 0.0f;
    }
    current_state_time_ += wall_dt;
  }
}

void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
{
  // Error check
  if (!robot_state_ || !robot_model_)
  {
    ROS_ERROR_STREAM_NAMED("trajectory_visualization","No robot state or robot model loaded");
    return;
  }

  if (!msg->model_id.empty() && msg->model_id != robot_model_->getName())
    ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected",
             msg->model_id.c_str(), robot_model_->getName().c_str());

  trajectory_message_to_display_.reset();

  robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
  for (std::size_t i = 0 ; i < msg->trajectory.size() ; ++i)
  {
    if (t->empty())
    {
      t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]);
    }
    else
    {
      robot_trajectory::RobotTrajectory tmp(robot_model_, "");
      tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]);
      t->append(tmp, 0.0);
    }
  }

  if (!t->empty())
  {
    trajectory_message_to_display_.swap(t);
  }
  if (trail_display_property_->getBool())
  {
    // incomingDisplayTrajectory() can be called from a non-GUI thread, so here we
    // use a signal/slot connection to invoke changedShowTrail() in the GUI thread.
    Q_EMIT timeToShowNewTrail();
  }
}

} // namespace moveit_rviz_plugin
<|endoftext|>


<commit_before>#include "yarrar/Pipeline.h"
#include "yarrar/dataprovider/WebcamDataProvider.h"
#include "yarrar/dataprovider/StaticImageDataProvider.h"
#include "yarrar/detector/MarkerDetector.h"
#include "yarrar/renderer/opengl/OpenGLRenderer.h"
#include "yarrar/renderer/dummy/DummyRenderer.h"
#include "yarrar/renderer/opencv/OpenCVRenderer.h"

int main(int argc, char** argv)
{
    using namespace yarrar;

    Pipeline p;
    p.addDataProvider<WebcamDataProvider>();
    //p.addDataProvider<StaticImageDataProvider>("/home/niki/programming/yarrar/data/img/marker.jpg");
    p.addDetector<MarkerDetector>();
    //p.addRenderer<DummyRenderer>();
    //p.addRenderer<OpenCVRenderer>();
    p.addRenderer<OpenGLRenderer>();

    while(true)
    {
        p.run();
        p.runRender();
        //std::getchar();
    }
}

<commit_msg>Clean up desktop example.<commit_after>#include "yarrar/Pipeline.h"
#include "yarrar/dataprovider/WebcamDataProvider.h"
#include "yarrar/detector/MarkerDetector.h"
#include "yarrar/renderer/opengl/OpenGLRenderer.h"

int main(int argc, char** argv)
{
    using namespace yarrar;

    Pipeline p;
    p.addDataProvider<WebcamDataProvider>();
    p.addDetector<MarkerDetector>();
    p.addRenderer<OpenGLRenderer>();

    while(true)
    {
        p.run();
        p.runRender();
    }
}

<|endoftext|>


<commit_before>/******************************************************************************\
* File:          app.cpp
* Purpose:       Implementation of sample classes for wxExRep
* Author:        Anton van Wezenbeek
* RCS-ID:        $Id$
*
* Copyright (c) 1998-2009, Anton van Wezenbeek
* All rights are reserved. Reproduction in whole or part is prohibited
* without the written consent of the copyright owner.
\******************************************************************************/

#include <wx/aboutdlg.h>
#include <wx/stdpaths.h> // for wxStandardPaths
#include <wx/extension/lexers.h>
#include <wx/extension/log.h>
#include <wx/extension/printing.h>
#include <wx/extension/util.h>
#include <wx/extension/version.h>
#include <wx/extension/report/dir.h>
#include <wx/extension/report/listitem.h>
#include <wx/extension/report/process.h>
#include "app.h"

#ifndef __WXMSW__
#include "app.xpm"
#else
#define _CRTDBG_MAP_ALLOC
#include <stdlib.h>
#include <crtdbg.h>
#endif

enum
{
  ID_PROCESS_DIALOG,
  ID_PROCESS_RUN,
  ID_RECENTFILE_MENU,
};

BEGIN_EVENT_TABLE(wxExRepSampleFrame, wxExFrameWithHistory)
  EVT_MENU(wxID_STOP, wxExRepSampleFrame::OnCommand)
  EVT_MENU(ID_PROCESS_DIALOG, wxExRepSampleFrame::OnCommand)
  EVT_MENU(ID_PROCESS_RUN, wxExRepSampleFrame::OnCommand)
  EVT_MENU_RANGE(wxID_CUT, wxID_CLEAR, wxExRepSampleFrame::OnCommand)
  EVT_MENU_RANGE(wxID_OPEN, wxID_PREFERENCES, wxExRepSampleFrame::OnCommand)
  EVT_TREE_ITEM_ACTIVATED(wxID_TREECTRL, wxExRepSampleFrame::OnTree)
END_EVENT_TABLE()

IMPLEMENT_APP(wxExRepSampleApp)

bool wxExRepSampleApp::OnInit()
{
  SetAppName("wxExRepSample");

  if (!wxExApp::OnInit())
  {
    return false;
  }

  wxExLog::Get()->SetLogging();

  wxExRepSampleFrame *frame = new wxExRepSampleFrame();

  frame->Show(true);

  SetTopWindow(frame);

  return true;
}

wxExRepSampleFrame::wxExRepSampleFrame()
  : wxExFrameWithHistory(NULL, wxID_ANY, wxTheApp->GetAppName())
{
  SetIcon(wxICON(app));

  wxExMenu *menuFile = new wxExMenu;
  menuFile->Append(wxID_OPEN);
  UseFileHistory(ID_RECENTFILE_MENU, menuFile);
  menuFile->AppendSeparator();
  menuFile->AppendPrint();
  menuFile->AppendSeparator();
  menuFile->Append(wxID_EXIT);

  wxExMenu *menuProcess = new wxExMenu;
  menuProcess->Append(ID_PROCESS_DIALOG, wxExEllipsed(_("Dialog")));
  menuProcess->Append(ID_PROCESS_RUN, _("Run"));
  menuProcess->AppendSeparator();
  menuProcess->Append(wxID_STOP);

  wxExMenu* menuHelp = new wxExMenu;
  menuHelp->Append(wxID_ABOUT);

  wxMenuBar *menubar = new wxMenuBar;
  menubar->Append(menuFile, _("&File"));
  menubar->Append(menuProcess, _("&Process"));
  menubar->Append(menuHelp, _("&Help"));
  SetMenuBar(menubar);

  CreateToolBar();

  m_ToolBar->AddTool(wxID_OPEN);
  m_ToolBar->Realize();

#if wxUSE_STATUSBAR
  std::vector<wxExPane> panes;
  panes.push_back(wxExPane("PaneText", -3));
  panes.push_back(wxExPane("PaneFileType", 50));
  panes.push_back(wxExPane("PaneLines", 100));
  panes.push_back(wxExPane("PaneLexer", 60));
  panes.push_back(wxExPane("PaneItems", 60));
  SetupStatusBar(panes);
#endif

  const wxExLexer lexer = wxExLexers::Get()->FindByName("cpp");

  m_DirCtrl = new wxGenericDirCtrl(
    this,
    wxID_ANY,
    wxStandardPaths::Get().GetDocumentsDir());

  m_NotebookWithLists = new wxExNotebook(
    this, this,
    wxID_ANY, wxDefaultPosition, wxDefaultSize,
    wxAUI_NB_DEFAULT_STYLE |
    wxAUI_NB_WINDOWLIST_BUTTON);

  m_STC = new wxExSTCWithFrame(this, this); // use all flags (default)

  for (
    int i = wxExListViewWithFrame::LIST_BEFORE_FIRST + 1;
    i < wxExListViewWithFrame::LIST_AFTER_LAST;
    i++)
  {
    wxExListViewWithFrame* vw = new wxExListViewWithFrame(
      this,
      this,
      (wxExListViewWithFrame::ListType)i,
      wxID_ANY,
      0xFF,
      &lexer); // set all flags

    m_NotebookWithLists->AddPage(
      vw,
      vw->GetTypeDescription(),
      vw->GetTypeDescription(),
      true);
  }

  GetManager().AddPane(
    m_STC,
    wxAuiPaneInfo().CenterPane().CloseButton(false).MaximizeButton(true));

  GetManager().AddPane(
    m_NotebookWithLists,
    wxAuiPaneInfo().CloseButton(false).Bottom().MinSize(wxSize(250, 250)));

  GetManager().AddPane(
    m_DirCtrl,
    wxAuiPaneInfo().Caption(_("DirCtrl")).Left().MinSize(wxSize(250, 250)));

  GetManager().Update();

  wxExDirWithListView dir(
    (wxExListViewWithFrame*)m_NotebookWithLists->GetPageByKey(
      wxExListViewWithFrame::GetTypeDescription(wxExListViewWithFrame::LIST_PROJECT)),
    wxGetCwd(),
    "*.cpp;*.h");

  dir.FindFiles();

  wxExListItem item(
    (wxExListViewWithFrame*)m_NotebookWithLists->GetPageByKey(
      wxExListViewWithFrame::GetTypeDescription(wxExListViewWithFrame::LIST_PROJECT)),
    "NOT EXISTING ITEM");

  item.Insert();
}

wxExListViewWithFrame* wxExRepSampleFrame::Activate(
  wxExListViewWithFrame::ListType type,
  const wxExLexer* lexer)
{
  for (
    size_t i = 0;
    i < m_NotebookWithLists->GetPageCount();
    i++)
  {
    wxExListViewWithFrame* vw = (wxExListViewWithFrame*)m_NotebookWithLists->GetPage(i);

    if (vw->GetType() == type)
    {
      if (type == wxExListViewWithFrame::LIST_KEYWORD)
      {
        if (lexer != NULL)
        {
          if (lexer->GetScintillaLexer() != "cpp")
          {
            wxLogMessage(lexer->GetScintillaLexer() + ", only cpp for the sample");
            return NULL;
          }
        }
      }

      return vw;
    }
  }

  return NULL;
}

wxExListView* wxExRepSampleFrame::GetListView()
{
  return (wxExListView*)m_NotebookWithLists->GetPage(
    m_NotebookWithLists->GetSelection());
}

wxExSTC* wxExRepSampleFrame::GetSTC()
{
  return m_STC;
}

void wxExRepSampleFrame::OnCommand(wxCommandEvent& event)
{
  switch (event.GetId())
  {
  case wxID_ABOUT:
    {
    wxAboutDialogInfo info;
    info.SetIcon(GetIcon());
    info.SetVersion(wxEX_VERSION_STRING);
    info.AddDeveloper(wxVERSION_STRING);
    info.SetCopyright("(c) 1998-2009 Anton van Wezenbeek");
    wxAboutBox(info);
    }
    break;

  case wxID_EXIT: Close(true); break;

  case wxID_OPEN:
    event.Skip();
    break;

  case wxID_PREVIEW:
    if (m_STC->HasCapture())
    {
      m_STC->PrintPreview();
    }
    else
    {
      wxExListView* lv = GetFocusedListView();

      if (lv != NULL)
      {
        lv->PrintPreview();
      }
    }
    break;

  case wxID_PRINT:
    {
      wxExListView* lv = GetFocusedListView();

      if (lv != NULL)
      {
        lv->Print();
      }
    }
    break;

  case wxID_PRINT_SETUP: wxExPrinting::Get()->GetHtmlPrinter()->PageSetup();
    break;

  case wxID_STOP:
    ProcessStop();
    break;

  case ID_PROCESS_DIALOG:
    wxExProcess::Get()->ConfigDialog(this);
    break;

  case ID_PROCESS_RUN:
    ProcessRun();
    break;

  default:
    wxFAIL;
    break;
  }
}

void wxExRepSampleFrame::OnTree(wxTreeEvent& /* event */)
{
  const wxString selection = m_DirCtrl->GetFilePath();

  if (!selection.empty())
  {
    OpenFile(wxExFileName(selection));
  }
}

bool wxExRepSampleFrame::OpenFile(const wxExFileName& file,
  int line_number,
  const wxString& match,
  long flags)
{
  // We cannot use the wxExFrameWithHistory::OpenFile, as that uses the focused STC.
  // Prevent recursion.
  if (flags & wxExSTC::STC_OPEN_FROM_OTHER)
  {
    flags = 0;
  }

  return m_STC->Open(file, line_number, match, flags);
}
<commit_msg>fixed compile error<commit_after>/******************************************************************************\
* File:          app.cpp
* Purpose:       Implementation of sample classes for wxExRep
* Author:        Anton van Wezenbeek
* RCS-ID:        $Id$
*
* Copyright (c) 1998-2009, Anton van Wezenbeek
* All rights are reserved. Reproduction in whole or part is prohibited
* without the written consent of the copyright owner.
\******************************************************************************/

#include <wx/aboutdlg.h>
#include <wx/stdpaths.h> // for wxStandardPaths
#include <wx/extension/lexers.h>
#include <wx/extension/log.h>
#include <wx/extension/printing.h>
#include <wx/extension/util.h>
#include <wx/extension/version.h>
#include <wx/extension/report/dir.h>
#include <wx/extension/report/listitem.h>
#include <wx/extension/report/process.h>
#include "app.h"

#ifndef __WXMSW__
#include "app.xpm"
#else
#define _CRTDBG_MAP_ALLOC
#include <stdlib.h>
#include <crtdbg.h>
#endif

enum
{
  ID_PROCESS_DIALOG,
  ID_PROCESS_RUN,
  ID_RECENTFILE_MENU,
};

BEGIN_EVENT_TABLE(wxExRepSampleFrame, wxExFrameWithHistory)
  EVT_MENU(wxID_STOP, wxExRepSampleFrame::OnCommand)
  EVT_MENU(ID_PROCESS_DIALOG, wxExRepSampleFrame::OnCommand)
  EVT_MENU(ID_PROCESS_RUN, wxExRepSampleFrame::OnCommand)
  EVT_MENU_RANGE(wxID_CUT, wxID_CLEAR, wxExRepSampleFrame::OnCommand)
  EVT_MENU_RANGE(wxID_OPEN, wxID_PREFERENCES, wxExRepSampleFrame::OnCommand)
  EVT_TREE_ITEM_ACTIVATED(wxID_TREECTRL, wxExRepSampleFrame::OnTree)
END_EVENT_TABLE()

IMPLEMENT_APP(wxExRepSampleApp)

bool wxExRepSampleApp::OnInit()
{
  SetAppName("wxExRepSample");

  if (!wxExApp::OnInit())
  {
    return false;
  }

  wxExLog::Get()->SetLogging();

  wxExRepSampleFrame *frame = new wxExRepSampleFrame();

  frame->Show(true);

  SetTopWindow(frame);

  return true;
}

wxExRepSampleFrame::wxExRepSampleFrame()
  : wxExFrameWithHistory(NULL, wxID_ANY, wxTheApp->GetAppName())
{
  SetIcon(wxICON(app));

  wxExMenu *menuFile = new wxExMenu;
  menuFile->Append(wxID_OPEN);
  UseFileHistory(ID_RECENTFILE_MENU, menuFile);
  menuFile->AppendSeparator();
  menuFile->AppendPrint();
  menuFile->AppendSeparator();
  menuFile->Append(wxID_EXIT);

  wxExMenu *menuProcess = new wxExMenu;
  menuProcess->Append(ID_PROCESS_DIALOG, wxExEllipsed(_("Dialog")));
  menuProcess->Append(ID_PROCESS_RUN, _("Run"));
  menuProcess->AppendSeparator();
  menuProcess->Append(wxID_STOP);

  wxExMenu* menuHelp = new wxExMenu;
  menuHelp->Append(wxID_ABOUT);

  wxMenuBar *menubar = new wxMenuBar;
  menubar->Append(menuFile, _("&File"));
  menubar->Append(menuProcess, _("&Process"));
  menubar->Append(menuHelp, _("&Help"));
  SetMenuBar(menubar);

  CreateToolBar();

  m_ToolBar->AddTool(wxID_OPEN);
  m_ToolBar->Realize();

#if wxUSE_STATUSBAR
  std::vector<wxExPane> panes;
  panes.push_back(wxExPane("PaneText", -3));
  panes.push_back(wxExPane("PaneFileType", 50));
  panes.push_back(wxExPane("PaneLines", 100));
  panes.push_back(wxExPane("PaneLexer", 60));
  panes.push_back(wxExPane("PaneItems", 60));
  SetupStatusBar(panes);
#endif

  const wxExLexer lexer = wxExLexers::Get()->FindByName("cpp");

  m_DirCtrl = new wxGenericDirCtrl(
    this,
    wxID_ANY,
    wxStandardPaths::Get().GetDocumentsDir());

  m_NotebookWithLists = new wxExNotebook(
    this, this,
    wxID_ANY, wxDefaultPosition, wxDefaultSize,
    wxAUI_NB_DEFAULT_STYLE |
    wxAUI_NB_WINDOWLIST_BUTTON);

  m_STC = new wxExSTCWithFrame(this, this); // use all flags (default)

  for (
    int i = wxExListViewWithFrame::LIST_BEFORE_FIRST + 1;
    i < wxExListViewWithFrame::LIST_AFTER_LAST;
    i++)
  {
    wxExListViewWithFrame* vw = new wxExListViewWithFrame(
      this,
      this,
      (wxExListViewWithFrame::ListType)i,
      wxID_ANY,
      0xFF,
      &lexer); // set all flags

    m_NotebookWithLists->AddPage(
      vw,
      vw->GetTypeDescription(),
      vw->GetTypeDescription(),
      true);
  }

  GetManager().AddPane(
    m_STC,
    wxAuiPaneInfo().CenterPane().CloseButton(false).MaximizeButton(true));

  GetManager().AddPane(
    m_NotebookWithLists,
    wxAuiPaneInfo().CloseButton(false).Bottom().MinSize(wxSize(250, 250)));

  GetManager().AddPane(
    m_DirCtrl,
    wxAuiPaneInfo().Caption(_("DirCtrl")).Left().MinSize(wxSize(250, 250)));

  GetManager().Update();

  wxExDirWithListView dir(
    (wxExListViewWithFrame*)m_NotebookWithLists->GetPageByKey(
      wxExListViewWithFrame::GetTypeDescription(wxExListViewWithFrame::LIST_PROJECT)),
    wxGetCwd(),
    "*.cpp;*.h");

  dir.FindFiles();

  wxExListItem item(
    (wxExListViewWithFrame*)m_NotebookWithLists->GetPageByKey(
      wxExListViewWithFrame::GetTypeDescription(wxExListViewWithFrame::LIST_PROJECT)),
    wxFileName("NOT EXISTING ITEM"));

  item.Insert();
}

wxExListViewWithFrame* wxExRepSampleFrame::Activate(
  wxExListViewWithFrame::ListType type,
  const wxExLexer* lexer)
{
  for (
    size_t i = 0;
    i < m_NotebookWithLists->GetPageCount();
    i++)
  {
    wxExListViewWithFrame* vw = (wxExListViewWithFrame*)m_NotebookWithLists->GetPage(i);

    if (vw->GetType() == type)
    {
      if (type == wxExListViewWithFrame::LIST_KEYWORD)
      {
        if (lexer != NULL)
        {
          if (lexer->GetScintillaLexer() != "cpp")
          {
            wxLogMessage(lexer->GetScintillaLexer() + ", only cpp for the sample");
            return NULL;
          }
        }
      }

      return vw;
    }
  }

  return NULL;
}

wxExListView* wxExRepSampleFrame::GetListView()
{
  return (wxExListView*)m_NotebookWithLists->GetPage(
    m_NotebookWithLists->GetSelection());
}

wxExSTC* wxExRepSampleFrame::GetSTC()
{
  return m_STC;
}

void wxExRepSampleFrame::OnCommand(wxCommandEvent& event)
{
  switch (event.GetId())
  {
  case wxID_ABOUT:
    {
    wxAboutDialogInfo info;
    info.SetIcon(GetIcon());
    info.SetVersion(wxEX_VERSION_STRING);
    info.AddDeveloper(wxVERSION_STRING);
    info.SetCopyright("(c) 1998-2009 Anton van Wezenbeek");
    wxAboutBox(info);
    }
    break;

  case wxID_EXIT: Close(true); break;

  case wxID_OPEN:
    event.Skip();
    break;

  case wxID_PREVIEW:
    if (m_STC->HasCapture())
    {
      m_STC->PrintPreview();
    }
    else
    {
      wxExListView* lv = GetFocusedListView();

      if (lv != NULL)
      {
        lv->PrintPreview();
      }
    }
    break;

  case wxID_PRINT:
    {
      wxExListView* lv = GetFocusedListView();

      if (lv != NULL)
      {
        lv->Print();
      }
    }
    break;

  case wxID_PRINT_SETUP: wxExPrinting::Get()->GetHtmlPrinter()->PageSetup();
    break;

  case wxID_STOP:
    ProcessStop();
    break;

  case ID_PROCESS_DIALOG:
    wxExProcess::Get()->ConfigDialog(this);
    break;

  case ID_PROCESS_RUN:
    ProcessRun();
    break;

  default:
    wxFAIL;
    break;
  }
}

void wxExRepSampleFrame::OnTree(wxTreeEvent& /* event */)
{
  const wxString selection = m_DirCtrl->GetFilePath();

  if (!selection.empty())
  {
    OpenFile(wxExFileName(selection));
  }
}

bool wxExRepSampleFrame::OpenFile(const wxExFileName& file,
  int line_number,
  const wxString& match,
  long flags)
{
  // We cannot use the wxExFrameWithHistory::OpenFile, as that uses the focused STC.
  // Prevent recursion.
  if (flags & wxExSTC::STC_OPEN_FROM_OTHER)
  {
    flags = 0;
  }

  return m_STC->Open(file, line_number, match, flags);
}
<|endoftext|>


<commit_before>// Virvo - Virtual Reality Volume Rendering
// Copyright (C) 1999-2003 University of Stuttgart, 2004-2005 Brown University
// Contact: Jurgen P. Schulze, jschulze@ucsd.edu
//
// This file is part of Virvo.
//
// Virvo 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 (see license.txt); if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

#include "vvcocoaglcontext.h"
#include "vvdebugmsg.h"
#include "vvrendercontext.h"
#include "vvx11.h"

#include <sstream>
#include <vector>

struct ContextArchData
{
#ifdef USE_COCOA
  vvCocoaGLContext* cocoaContext;
#endif

#ifdef USE_X11
  GLXContext glxContext;
  Display* display;
  Drawable drawable;
  std::vector<int> attributes;
  GLXFBConfig* fbConfigs;
#endif
};

vvRenderContext::vvRenderContext(vvContextOptions* co)
  : _options(co)
{
  _archData = new ContextArchData;
  _initialized = false;
  init();
}

vvRenderContext::~vvRenderContext()
{
  vvDebugMsg::msg(1, "vvRenderContext::~vvRenderContext");

  if (_initialized)
  {
#ifdef USE_COCOA
    delete _archData->cocoaContext;
#endif

#ifdef USE_X11
    glXDestroyContext(_archData->display, _archData->glxContext);
    XCloseDisplay(_archData->display);
#endif
  }
  delete _archData;
}

bool vvRenderContext::makeCurrent() const
{
  if (_initialized)
  {
#ifdef USE_COCOA
    return _archData->cocoaContext->makeCurrent();
#endif

#if USE_X11
    return glXMakeCurrent(_archData->display, _archData->drawable, _archData->glxContext);
#endif
  }
  return false;
}

void vvRenderContext::swapBuffers() const
{
  if (_initialized)
  {
#ifdef USE_COCOA
    _archData->cocoaContext->swapBuffers();
#endif

#ifdef USE_X11
    glXSwapBuffers(_archData->display, _archData->drawable);
#endif
  }
}

void vvRenderContext::resize(const int w, const int h)
{
  if ((_options->width != w) || (_options->height != h))
  {
    _options->width = w;
    _options->height = h;
    if (_initialized)
    {
#ifdef USE_COCOA
      _archData->cocoaContext->resize(w, h);
#endif

#ifdef USE_X11
      switch (_options->type)
      {
      case vvContextOptions::VV_PBUFFER:
      {
        glXDestroyPbuffer(_archData->display, _archData->drawable);
        initPbuffer();
        makeCurrent();
        break;
      }
      case vvContextOptions::VV_WINDOW:
        // fall through
      default:
        XResizeWindow(_archData->display, _archData->drawable,
                      static_cast<uint>(w),
                      static_cast<uint>(h));
        XSync(_archData->display, False);
        break;
      }
#endif
    }
  }
}

void vvRenderContext::init()
{
#ifdef USE_COCOA
  _archData->cocoaContext = new vvCocoaGLContext(_options);
  _initialized = true;
#endif

#ifdef USE_X11
  _archData->display = XOpenDisplay(_options->displayName.c_str());

  _archData->attributes.push_back(GLX_RGBA);
  _archData->attributes.push_back(GLX_RED_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_GREEN_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_BLUE_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_ALPHA_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_DEPTH_SIZE);
  _archData->attributes.push_back(24);
  _archData->attributes.push_back(None);

  if(_archData->display != NULL)
  {
    switch(_options->type)
    {
    case vvContextOptions::VV_PBUFFER:
      if (initPbuffer())
      {
         _archData->glxContext = glXCreateNewContext(_archData->display, _archData->fbConfigs[0], GLX_RGBA_TYPE, 0, True);
        _initialized = true;
        return;
      }
      else
      {
        _options->type = vvContextOptions::VV_WINDOW;
      }
      // no pbuffer created - fall through
    case vvContextOptions::VV_WINDOW:
      // fall through
    default:
      {
        const Drawable parent = RootWindow(_archData->display, DefaultScreen(_archData->display));

        XVisualInfo* vi = glXChooseVisual(_archData->display,
                                          DefaultScreen(_archData->display),
                                          &(_archData->attributes)[0]);

        XSetWindowAttributes wa = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
        wa.colormap = XCreateColormap(_archData->display, parent, vi->visual, AllocNone);
        wa.background_pixmap = None;
        wa.border_pixel = 0;

        if (vvDebugMsg::getDebugLevel() == 0)
        {
          wa.override_redirect = true;
        }
        else
        {
          wa.override_redirect = false;
        }

        _archData->glxContext = glXCreateContext(_archData->display, vi, NULL, True);

        if (_archData->glxContext != 0)
        {

          int windowWidth = _options->width;
          int windowHeight = _options->height;
          if (vvDebugMsg::getDebugLevel() > 0)
          {
            windowWidth = 512;
            windowHeight = 512;
          }

          _archData->drawable = XCreateWindow(_archData->display, parent, 0, 0, windowWidth, windowHeight, 0,
                                              vi->depth, InputOutput, vi->visual,
                                              CWBackPixmap|CWBorderPixel|CWColormap|CWOverrideRedirect, &wa);
          XMapWindow(_archData->display, _archData->drawable);
          XFlush(_archData->display);
          _initialized = true;
        }
        else
        {
          vvDebugMsg::msg( 0, "Couldn't create OpenGL context");
          _initialized = false;
        }
        _initialized = true;
        delete vi;
        break;
      }
    }
  }
  else
  {
    _initialized = false;
    std::ostringstream errmsg;
    errmsg << "vvRenderContext::init() error: Could not open display " << _options->displayName;
    vvDebugMsg::msg(0, errmsg.str().c_str());
  }
#endif
}

bool vvRenderContext::initPbuffer()
{
#ifdef USE_X11
  int nelements;
  _archData->fbConfigs = glXChooseFBConfig(_archData->display, DefaultScreen(_archData->display),
                                           &(_archData->attributes)[1], &nelements); // first entry (GLX_RGBA) in attributes list confuses pbuffers
  if ((_archData->fbConfigs != NULL) && (nelements > 0))
  {
    // TODO: find the nicest fbconfig.
    int pbAttrList[] = { GLX_PBUFFER_WIDTH, _options->width, GLX_PBUFFER_HEIGHT, _options->height, None };
    _archData->drawable = glXCreatePbuffer(_archData->display, _archData->fbConfigs[0], pbAttrList);
    if (!_archData->drawable)
    {
      std::cerr << "No pbuffer created" << std::endl;
      return false;
    }
    return true;
  }
#endif
  return false;
}
// vim: sw=2:expandtab:softtabstop=2:ts=2:cino=\:0g0t0

<commit_msg>Cleanup render contexts<commit_after>// Virvo - Virtual Reality Volume Rendering
// Copyright (C) 1999-2003 University of Stuttgart, 2004-2005 Brown University
// Contact: Jurgen P. Schulze, jschulze@ucsd.edu
//
// This file is part of Virvo.
//
// Virvo 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 (see license.txt); if not, write to the
// Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

#include "vvcocoaglcontext.h"
#include "vvdebugmsg.h"
#include "vvrendercontext.h"
#include "vvx11.h"

#include <sstream>
#include <vector>

struct ContextArchData
{
#ifdef USE_COCOA
  vvCocoaGLContext* cocoaContext;
#endif

#ifdef USE_X11
  GLXContext glxContext;
  Display* display;
  Drawable drawable;
  std::vector<int> attributes;
  GLXFBConfig* fbConfigs;
#endif
};

vvRenderContext::vvRenderContext(vvContextOptions* co)
  : _options(co)
{
  _archData = new ContextArchData;
  _initialized = false;
  init();
}

vvRenderContext::~vvRenderContext()
{
  vvDebugMsg::msg(1, "vvRenderContext::~vvRenderContext");

  if (_initialized)
  {
#ifdef USE_COCOA
    delete _archData->cocoaContext;
#endif

#ifdef USE_X11
    glXDestroyContext(_archData->display, _archData->glxContext);
    switch (_options->type)
    {
    case vvContextOptions::VV_PBUFFER:
      glXDestroyPbuffer(_archData->display, _archData->drawable);
      break;
    case vvContextOptions::VV_WINDOW:
      // fall through
    default:
      XDestroyWindow(_archData->display, _archData->drawable);
      break;
    }
    XCloseDisplay(_archData->display);
#endif
  }
  delete _archData;
}

bool vvRenderContext::makeCurrent() const
{
  if (_initialized)
  {
#ifdef USE_COCOA
    return _archData->cocoaContext->makeCurrent();
#endif

#if USE_X11
    return glXMakeCurrent(_archData->display, _archData->drawable, _archData->glxContext);
#endif
  }
  return false;
}

void vvRenderContext::swapBuffers() const
{
  if (_initialized)
  {
#ifdef USE_COCOA
    _archData->cocoaContext->swapBuffers();
#endif

#ifdef USE_X11
    glXSwapBuffers(_archData->display, _archData->drawable);
#endif
  }
}

void vvRenderContext::resize(const int w, const int h)
{
  if ((_options->width != w) || (_options->height != h))
  {
    _options->width = w;
    _options->height = h;
    if (_initialized)
    {
#ifdef USE_COCOA
      _archData->cocoaContext->resize(w, h);
#endif

#ifdef USE_X11
      switch (_options->type)
      {
      case vvContextOptions::VV_PBUFFER:
      {
        glXDestroyPbuffer(_archData->display, _archData->drawable);
        initPbuffer();
        makeCurrent();
        break;
      }
      case vvContextOptions::VV_WINDOW:
        // fall through
      default:
        XResizeWindow(_archData->display, _archData->drawable,
                      static_cast<uint>(w),
                      static_cast<uint>(h));
        XSync(_archData->display, False);
        break;
      }
#endif
    }
  }
}

void vvRenderContext::init()
{
#ifdef USE_COCOA
  _archData->cocoaContext = new vvCocoaGLContext(_options);
  _initialized = true;
#endif

#ifdef USE_X11
  _archData->display = XOpenDisplay(_options->displayName.c_str());

  _archData->attributes.push_back(GLX_RGBA);
  _archData->attributes.push_back(GLX_RED_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_GREEN_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_BLUE_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_ALPHA_SIZE);
  _archData->attributes.push_back(8);
  _archData->attributes.push_back(GLX_DEPTH_SIZE);
  _archData->attributes.push_back(24);
  _archData->attributes.push_back(None);

  if(_archData->display != NULL)
  {
    switch(_options->type)
    {
    case vvContextOptions::VV_PBUFFER:
      if (initPbuffer())
      {
         _archData->glxContext = glXCreateNewContext(_archData->display, _archData->fbConfigs[0], GLX_RGBA_TYPE, 0, True);
        _initialized = true;
        return;
      }
      else
      {
        _options->type = vvContextOptions::VV_WINDOW;
      }
      // no pbuffer created - fall through
    case vvContextOptions::VV_WINDOW:
      // fall through
    default:
      {
        const Drawable parent = RootWindow(_archData->display, DefaultScreen(_archData->display));

        XVisualInfo* vi = glXChooseVisual(_archData->display,
                                          DefaultScreen(_archData->display),
                                          &(_archData->attributes)[0]);

        XSetWindowAttributes wa = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
        wa.colormap = XCreateColormap(_archData->display, parent, vi->visual, AllocNone);
        wa.background_pixmap = None;
        wa.border_pixel = 0;

        if (vvDebugMsg::getDebugLevel() == 0)
        {
          wa.override_redirect = true;
        }
        else
        {
          wa.override_redirect = false;
        }

        _archData->glxContext = glXCreateContext(_archData->display, vi, NULL, True);

        if (_archData->glxContext != 0)
        {

          int windowWidth = _options->width;
          int windowHeight = _options->height;
          if (vvDebugMsg::getDebugLevel() > 0)
          {
            windowWidth = 512;
            windowHeight = 512;
          }

          _archData->drawable = XCreateWindow(_archData->display, parent, 0, 0, windowWidth, windowHeight, 0,
                                              vi->depth, InputOutput, vi->visual,
                                              CWBackPixmap|CWBorderPixel|CWColormap|CWOverrideRedirect, &wa);
          XMapWindow(_archData->display, _archData->drawable);
          XFlush(_archData->display);
          _initialized = true;
        }
        else
        {
          vvDebugMsg::msg( 0, "Couldn't create OpenGL context");
          _initialized = false;
        }
        _initialized = true;
        delete vi;
        break;
      }
    }
  }
  else
  {
    _initialized = false;
    std::ostringstream errmsg;
    errmsg << "vvRenderContext::init() error: Could not open display " << _options->displayName;
    vvDebugMsg::msg(0, errmsg.str().c_str());
  }
#endif
}

bool vvRenderContext::initPbuffer()
{
#ifdef USE_X11
  int nelements;
  _archData->fbConfigs = glXChooseFBConfig(_archData->display, DefaultScreen(_archData->display),
                                           &(_archData->attributes)[1], &nelements); // first entry (GLX_RGBA) in attributes list confuses pbuffers
  if ((_archData->fbConfigs != NULL) && (nelements > 0))
  {
    // TODO: find the nicest fbconfig.
    int pbAttrList[] = { GLX_PBUFFER_WIDTH, _options->width, GLX_PBUFFER_HEIGHT, _options->height, None };
    _archData->drawable = glXCreatePbuffer(_archData->display, _archData->fbConfigs[0], pbAttrList);
    if (!_archData->drawable)
    {
      std::cerr << "No pbuffer created" << std::endl;
      return false;
    }
    return true;
  }
#endif
  return false;
}
// vim: sw=2:expandtab:softtabstop=2:ts=2:cino=\:0g0t0

<|endoftext|>


<commit_before>#include <iostream>
using namespace std;

#include <TFile.h>
#include <TTree.h>
#include <TChain.h>
#include <TVectorD.h>
#include <TF1.h>

#include "X.h"
using namespace GeFiCa;
//______________________________________________________________________________
// Calculation of 1D static electronic potential and field.
// Successive Over-Relaxation (SOR) method is used to calculate the field in a
// grid. Please refer to https://mediatum.ub.tum.de/node?id=969435 for detailed
// description of SOR method. It will have some error compared with actual data
// but should be close.  Analytic solution is also provided for comparison.
ClassImp(X)
X::X(int nx) : TObject(), MaxIterations(100000), Csor(1), Precision(1e-7),
   fIsFixed(0), fE1(0), fPotential(0), fC1(0), fDistanceToNext(0), fDistanceToPrevious(0), fImpurity(0)
{
  //claim a 1D field with nx grisd
   n=nx;
   n1=nx;
   fIsLoaded=false;
   if (n<10) { n=11; n1=11; }
   fE1=new double[n];
   fC1=new double[n];
   fPotential=new double[n];
   fIsFixed=new bool[n];
   fDistanceToNext=new double[n];
   fDistanceToPrevious=new double[n];
   fImpurity=new double[n];
}

X::~X()
{
   if (fE1) delete[] fE1;
   if (fPotential) delete[] fPotential;
   if (fC1) delete[] fC1;
   if (fDistanceToNext) delete[] fDistanceToNext;
   if (fDistanceToPrevious) delete[] fDistanceToPrevious;
   if (fIsFixed) delete[] fIsFixed;
   if (fImpurity) delete[] fImpurity;
}

bool X::Analytic()
{
  // Analytic calculation
   cout<<"no method can use"<<endl;
   return false;
}

void X::SetStepLength(double steplength)
{
//set field step length
   for (int i=n;i-->0;) {
      fIsFixed[i]=false;
      fE1[i]=0;
      fC1[i]=i*steplength;
      fPotential[i]=0;
      fDistanceToNext[i]=steplength;
      fDistanceToPrevious[i]=steplength;
      fImpurity[i]=0;
   }
}

bool X::CalculateField(EMethod method)
{
  //do calculate

  fIsLoaded=true;

   if (method==kAnalytic) return Analytic();
   int cnt=0;
   while (cnt++<MaxIterations) {
      double XUpSum=0;
      double XDownSum=0;
      for (int i=1;i<n-1;i++) {
         double old=fPotential[i];
         SOR2(i,0);
         if(old>0)XDownSum+=old;
         else XDownSum-=old;
         if(fPotential[i]-old>0)XUpSum+=(fPotential[i]-old);
         else XUpSum+=-(fPotential[i]-old);
      }
      if(cnt%1000==0)
         cout<<cnt<<"  "<<XUpSum/XDownSum<<" down: "<<XDownSum<<", up: "<<XUpSum<<endl;
      if (XUpSum/XDownSum<Precision){

	  return true;
      }
   }
   return false;
}

void X::SOR2(int idx,bool elec)
{
  // 2nd-order Runge-Kutta Successive Over-Relaxation
   if (fIsFixed[idx])return ;
   double density=fImpurity[idx]*1.6e-19;
   double h2=fDistanceToPrevious[idx];
   double h3=fDistanceToNext[idx];
   double tmp=-density/epsilon*h2*h3/2+(h3*fPotential[idx-1]+h2*fPotential[idx+1])/(h2+h3);
   // over-relaxation if Csor>1
   fPotential[idx]=Csor*(tmp-fPotential[idx])+fPotential[idx];

   if(elec)fE1[idx]=(fPotential[idx+1]-fPotential[idx-1])/(h2+h3);
}

int X::FindIdx(double tarx,int begin,int end)
{
  //search using binary search
   if (begin>=end)return begin;
   int mid=(begin+end)/2;
   if(fC1[mid]>=tarx)return FindIdx(tarx,begin,mid);
   else return FindIdx(tarx,mid+1,end);
}

double X::GetXEdge(bool beginorend)
{
  //true if end, false if end
  if(beginorend)return fC1[n1-1];
  if(!beginorend)return fC1[0];
}
double X::GetData(double tarx,int thing)
{
  // ask thingwith number: 1:Potential 2:E1 0:Impurty
   int idx=FindIdx(tarx,0,n-1);
   if (idx==n)
   {
      switch (thing)
      {
         case 0:return fImpurity[idx];
         case 2:return fE1[idx];
         case 1:return fPotential[idx];
      }
   }
   double ab=(tarx-fC1[idx])/fDistanceToNext[idx];
   double aa=1-ab;
   switch(thing)
   {
      case 2:return fE1[idx]*ab+fE1[idx+1]*aa;
      case 1:return fPotential[idx]*ab+fC1[idx+1]*aa;
      case 0:return fImpurity[idx]*ab+fImpurity[idx+1]*aa;
   }
   return -1;
}
void X::SaveField(const char * fout)
{
   TFile * file=new TFile(fout,"recreate","data");
   TTree * tree=new TTree("t","1D");

   TVectorD v(10);

   v[7]=(double)n1;
   v[8]=1;;
   v[9]=1;
   v[0]=(double)MaxIterations;
   v[1]=(double)n;
   v[2]=Csor;
   v.Write("v");
   bool fIsFixeds;

   double E1s,C1s,Ps,StepNexts,StepBefores,impuritys;
   tree->Branch("e1",&E1s,"e1/D"); // Electric X in x
   tree->Branch("c1",&C1s,"c1/D"); // persition in x
   tree->Branch("p",&Ps,"p/D"); // electric potential
   tree->Branch("sn",&StepNexts,"StepNext/D"); // Step length to next point in x
   tree->Branch("sb",&StepBefores,"Step)Before/D"); // Step length to before point in x
   tree->Branch("ib",&fIsFixeds,"fIsFixed/O"); // check is initial point
   tree->Branch("im",&impuritys,"impurity/D"); // Impurity
   for(int i=0;i<n;i++) {
      impuritys=fImpurity[i];
      E1s=fE1[i];
      C1s=fC1[i];
      Ps=fPotential[i];
      StepNexts=fDistanceToNext[i];
      StepBefores=fDistanceToPrevious[i];
      tree->Fill();
   }
   file->Write();
   file->Close();
   delete file;
}
void X::LoadField(const char * fin)
{
  //will calculate electric field after load
  fIsLoaded=true;
   TFile *file=new TFile(fin);
   TVectorD *v1=(TVectorD*)file->Get("v");
   double * v=v1->GetMatrixArray();
   n1		=(int)	v[7];
   MaxIterations	=(int)	v[0];
   n		=(int)	v[1];
   Csor		=	v[2];

   TChain *t =new TChain("t");
   t->Add(fin);
   bool IsFixed;
   double E1,C1,fP,fStepNext,fStepBefore,fimpurity;
   t->SetBranchAddress("e1",&E1);
   t->SetBranchAddress("c1",&C1);
   t->SetBranchAddress("p",&fP);
   t->SetBranchAddress("sn",&fStepNext);
   t->SetBranchAddress("sb",&fStepBefore);
   t->SetBranchAddress("e1",&fE1);
   t->SetBranchAddress("if",&IsFixed);
   t->SetBranchAddress("im",&fimpurity);

   fE1=new double[n];
   fC1=new double[n];
   fPotential=new double[n];
   fIsFixed=new bool[n];
   fDistanceToNext=new double[n];
   fDistanceToPrevious=new double[n];
   fImpurity=new double[n];

   for (int i=0;i<n;i++) {
      t->GetEntry(i);
      fE1[i]=E1;
      fC1[i]=C1;
      fPotential[i]=fP;
      fIsFixed[i]=fIsFixed;
      fDistanceToNext[i]=fStepNext;
      fDistanceToPrevious[i]=fStepBefore;
      fImpurity[i]=fimpurity;
   }
   file->Close();
   delete file;
   for(int idx=0;idx-->n;)SOR2(idx,1);
}

void X::SetImpurity(double density)
{
   for(int i=n;i-->0;) fImpurity[i]=density;
}

void X::SetImpurity(TF1 * Im)
{
   for(int i=n;i-->0;) {
      fImpurity[i]=Im->Eval((double)fC1[i]);
   }
}

<commit_msg>fixed a comment typo<commit_after>#include <iostream>
using namespace std;

#include <TFile.h>
#include <TTree.h>
#include <TChain.h>
#include <TVectorD.h>
#include <TF1.h>

#include "X.h"
using namespace GeFiCa;
//______________________________________________________________________________
// Calculation of 1D static electronic potential and field.
// Successive Over-Relaxation (SOR) method is used to calculate the field in a
// grid. Please refer to https://mediatum.ub.tum.de/node?id=969435 for detailed
// description of SOR method. It will have some error compared with actual data
// but should be close.  Analytic solution is also provided for comparison.
ClassImp(X)
X::X(int nx) : TObject(), MaxIterations(100000), Csor(1), Precision(1e-7),
   fIsFixed(0), fE1(0), fPotential(0), fC1(0), fDistanceToNext(0), fDistanceToPrevious(0), fImpurity(0)
{
  //claim a 1D field with nx grids
   n=nx;
   n1=nx;
   fIsLoaded=false;
   if (n<10) { n=11; n1=11; }
   fE1=new double[n];
   fC1=new double[n];
   fPotential=new double[n];
   fIsFixed=new bool[n];
   fDistanceToNext=new double[n];
   fDistanceToPrevious=new double[n];
   fImpurity=new double[n];
}

X::~X()
{
   if (fE1) delete[] fE1;
   if (fPotential) delete[] fPotential;
   if (fC1) delete[] fC1;
   if (fDistanceToNext) delete[] fDistanceToNext;
   if (fDistanceToPrevious) delete[] fDistanceToPrevious;
   if (fIsFixed) delete[] fIsFixed;
   if (fImpurity) delete[] fImpurity;
}

bool X::Analytic()
{
  // Analytic calculation
   cout<<"no method can use"<<endl;
   return false;
}

void X::SetStepLength(double steplength)
{
//set field step length
   for (int i=n;i-->0;) {
      fIsFixed[i]=false;
      fE1[i]=0;
      fC1[i]=i*steplength;
      fPotential[i]=0;
      fDistanceToNext[i]=steplength;
      fDistanceToPrevious[i]=steplength;
      fImpurity[i]=0;
   }
}

bool X::CalculateField(EMethod method)
{
  //do calculate

  fIsLoaded=true;

   if (method==kAnalytic) return Analytic();
   int cnt=0;
   while (cnt++<MaxIterations) {
      double XUpSum=0;
      double XDownSum=0;
      for (int i=1;i<n-1;i++) {
         double old=fPotential[i];
         SOR2(i,0);
         if(old>0)XDownSum+=old;
         else XDownSum-=old;
         if(fPotential[i]-old>0)XUpSum+=(fPotential[i]-old);
         else XUpSum+=-(fPotential[i]-old);
      }
      if(cnt%1000==0)
         cout<<cnt<<"  "<<XUpSum/XDownSum<<" down: "<<XDownSum<<", up: "<<XUpSum<<endl;
      if (XUpSum/XDownSum<Precision){

	  return true;
      }
   }
   return false;
}

void X::SOR2(int idx,bool elec)
{
  // 2nd-order Runge-Kutta Successive Over-Relaxation
   if (fIsFixed[idx])return ;
   double density=fImpurity[idx]*1.6e-19;
   double h2=fDistanceToPrevious[idx];
   double h3=fDistanceToNext[idx];
   double tmp=-density/epsilon*h2*h3/2+(h3*fPotential[idx-1]+h2*fPotential[idx+1])/(h2+h3);
   // over-relaxation if Csor>1
   fPotential[idx]=Csor*(tmp-fPotential[idx])+fPotential[idx];

   if(elec)fE1[idx]=(fPotential[idx+1]-fPotential[idx-1])/(h2+h3);
}

int X::FindIdx(double tarx,int begin,int end)
{
  //search using binary search
   if (begin>=end)return begin;
   int mid=(begin+end)/2;
   if(fC1[mid]>=tarx)return FindIdx(tarx,begin,mid);
   else return FindIdx(tarx,mid+1,end);
}

double X::GetXEdge(bool beginorend)
{
  //true if end, false if end
  if(beginorend)return fC1[n1-1];
  if(!beginorend)return fC1[0];
}
double X::GetData(double tarx,int thing)
{
  // ask thingwith number: 1:Potential 2:E1 0:Impurty
   int idx=FindIdx(tarx,0,n-1);
   if (idx==n)
   {
      switch (thing)
      {
         case 0:return fImpurity[idx];
         case 2:return fE1[idx];
         case 1:return fPotential[idx];
      }
   }
   double ab=(tarx-fC1[idx])/fDistanceToNext[idx];
   double aa=1-ab;
   switch(thing)
   {
      case 2:return fE1[idx]*ab+fE1[idx+1]*aa;
      case 1:return fPotential[idx]*ab+fC1[idx+1]*aa;
      case 0:return fImpurity[idx]*ab+fImpurity[idx+1]*aa;
   }
   return -1;
}
void X::SaveField(const char * fout)
{
   TFile * file=new TFile(fout,"recreate","data");
   TTree * tree=new TTree("t","1D");

   TVectorD v(10);

   v[7]=(double)n1;
   v[8]=1;;
   v[9]=1;
   v[0]=(double)MaxIterations;
   v[1]=(double)n;
   v[2]=Csor;
   v.Write("v");
   bool fIsFixeds;

   double E1s,C1s,Ps,StepNexts,StepBefores,impuritys;
   tree->Branch("e1",&E1s,"e1/D"); // Electric X in x
   tree->Branch("c1",&C1s,"c1/D"); // persition in x
   tree->Branch("p",&Ps,"p/D"); // electric potential
   tree->Branch("sn",&StepNexts,"StepNext/D"); // Step length to next point in x
   tree->Branch("sb",&StepBefores,"Step)Before/D"); // Step length to before point in x
   tree->Branch("ib",&fIsFixeds,"fIsFixed/O"); // check is initial point
   tree->Branch("im",&impuritys,"impurity/D"); // Impurity
   for(int i=0;i<n;i++) {
      impuritys=fImpurity[i];
      E1s=fE1[i];
      C1s=fC1[i];
      Ps=fPotential[i];
      StepNexts=fDistanceToNext[i];
      StepBefores=fDistanceToPrevious[i];
      tree->Fill();
   }
   file->Write();
   file->Close();
   delete file;
}
void X::LoadField(const char * fin)
{
  //will calculate electric field after load
  fIsLoaded=true;
   TFile *file=new TFile(fin);
   TVectorD *v1=(TVectorD*)file->Get("v");
   double * v=v1->GetMatrixArray();
   n1		=(int)	v[7];
   MaxIterations	=(int)	v[0];
   n		=(int)	v[1];
   Csor		=	v[2];

   TChain *t =new TChain("t");
   t->Add(fin);
   bool IsFixed;
   double E1,C1,fP,fStepNext,fStepBefore,fimpurity;
   t->SetBranchAddress("e1",&E1);
   t->SetBranchAddress("c1",&C1);
   t->SetBranchAddress("p",&fP);
   t->SetBranchAddress("sn",&fStepNext);
   t->SetBranchAddress("sb",&fStepBefore);
   t->SetBranchAddress("e1",&fE1);
   t->SetBranchAddress("if",&IsFixed);
   t->SetBranchAddress("im",&fimpurity);

   fE1=new double[n];
   fC1=new double[n];
   fPotential=new double[n];
   fIsFixed=new bool[n];
   fDistanceToNext=new double[n];
   fDistanceToPrevious=new double[n];
   fImpurity=new double[n];

   for (int i=0;i<n;i++) {
      t->GetEntry(i);
      fE1[i]=E1;
      fC1[i]=C1;
      fPotential[i]=fP;
      fIsFixed[i]=fIsFixed;
      fDistanceToNext[i]=fStepNext;
      fDistanceToPrevious[i]=fStepBefore;
      fImpurity[i]=fimpurity;
   }
   file->Close();
   delete file;
   for(int idx=0;idx-->n;)SOR2(idx,1);
}

void X::SetImpurity(double density)
{
   for(int i=n;i-->0;) fImpurity[i]=density;
}

void X::SetImpurity(TF1 * Im)
{
   for(int i=n;i-->0;) {
      fImpurity[i]=Im->Eval((double)fC1[i]);
   }
}

<|endoftext|>