[simspark-cvs] simspark/contrib/rsgedit kinematicframe.cpp, NONE, 1.1 kinematicframe.h, NONE, 1.1

[Markus R. <rol...@us...>]

Update of /cvsroot/simspark/simspark/contrib/rsgedit
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv15903

Added Files:
	kinematicframe.cpp kinematicframe.h 
Log Message:
- initial minimal version of a frame that allows direct user control of joints. For all joint nodes below a given parrent node a slider is created that displays the current joint angle and allows the user to manipulate it.

--- NEW FILE: kinematicframe.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: kinematicframe.h,v 1.1 2007/07/08 13:44:17 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
// -*- C++ -*- generated by wxGlade 0.4.1 on Fri Feb 23 21:06:30 2007
#ifndef KINEMATICFRAME_H
#define KINEMATICFRAME_H

#include <wx/wx.h>
#include <zeitgeist/leaf.h>


class kinematicFrame: public wxFrame {
public:
    enum EControlMode
    {
        CM_PASSIV,
        CM_ACTIVE
    };

    struct JointControl
    {
    public:
        int type;
        int axis;
        boost::weak_ptr<zeitgeist::Leaf> joint;

        EControlMode mode;
        int target;

    public:
        JointControl()
            : type(0), axis(0), mode(CM_PASSIV), target(0)
        {
        }
    };

    typedef std::map<wxSlider*, JointControl> TJointControlMap;

public:
    kinematicFrame(wxWindow* parent, int id, const wxString& title, const wxPoint& pos=wxDefaultPosition,
                   const wxSize& size=wxDefaultSize, long style=wxDEFAULT_FRAME_STYLE);
    virtual ~kinematicFrame();

    /** set the parent node */
    void SetParent(boost::weak_ptr<zeitgeist::Leaf> parent);

    /** update the cached reference to the parent and recreate
        kinematic controls
    */
    void UpdateCached();

    /** update the joint sliders */
    void RefreshProperties();

protected:
    void OnSliderChanged(wxCommandEvent& event);
    void OnScrollChanged(wxScrollEvent& event);

    float GetJointVel(float diff);
    void MoveJoint(JointControl& entry);

protected:
    DECLARE_EVENT_TABLE()

    zeitgeist::Leaf::CachedPath<zeitgeist::Leaf> mParent;
    wxScrolledWindow* mCtrScrollWnd;
    TJointControlMap mJoints;

}; // wxGlade: end class

#endif // KINEMATICFRAME_H

--- NEW FILE: kinematicframe.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2003 Koblenz University
   $Id: kinematicframe.cpp,v 1.1 2007/07/08 13:44:17 rollmark Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
// -*- C++ -*- generated by wxGlade 0.4.1 on Fri Feb 23 21:06:30 2007
#include "main.h"
#include "kinematicframe.h"
#include "simspark.h"

//! wxWidgets and zeitgeist both use a 'DECLARE_CLASS' macro
#undef DECLARE_CLASS

#include <salt/gmath.h>
#include <oxygen/physicsserver/joint.h>
#include <oxygen/physicsserver/universaljoint.h>
#include <oxygen/physicsserver/sliderjoint.h>
#include <oxygen/physicsserver/hingejoint.h>

BEGIN_EVENT_TABLE(kinematicFrame, wxFrame)
    EVT_SLIDER( wxID_ANY, kinematicFrame::OnSliderChanged)
    EVT_SCROLL_CHANGED( kinematicFrame::OnScrollChanged)
END_EVENT_TABLE()

using namespace boost;
using namespace zeitgeist;
using namespace oxygen;
using namespace salt;

static const float MIN_JOINT_DIFF = 2;
static const float MAX_JOINT_VEL = 50;

kinematicFrame::kinematicFrame(wxWindow* parent, int id, const wxString& title, const wxPoint& pos, const wxSize& size, long style):
    wxFrame(parent, id, title, pos, wxSize(640,-1),
            wxCAPTION|wxCLOSE_BOX|wxSYSTEM_MENU|wxRESIZE_BORDER|wxFRAME_TOOL_WINDOW|wxFRAME_NO_TASKBAR|wxFRAME_FLOAT_ON_PARENT|wxCLIP_CHILDREN)
{
    SetTitle(wxT("Kinematics"));

    wxBoxSizer* topSizer = new wxBoxSizer(wxVERTICAL);
    this->SetSizer(topSizer);

    mCtrScrollWnd = new wxScrolledWindow( this, wxID_ANY, wxDefaultPosition, wxSize(-1,-1), wxNO_BORDER|wxHSCROLL|wxVSCROLL );
    topSizer->Add(mCtrScrollWnd, 1, wxGROW, 0);
    mCtrScrollWnd->SetScrollbars(1, 1, 0, 0);

    int rows = 0; // dynamic
    int cols = 2;
    int vgap = 0;
    int hgap = 0;

    wxFlexGridSizer* sizer = new wxFlexGridSizer(rows, cols, vgap, hgap);
    sizer->AddGrowableCol(1);
    sizer->SetFlexibleDirection(wxHORIZONTAL);
    mCtrScrollWnd->SetSizer(sizer);
}

kinematicFrame::~kinematicFrame()
{
}

void kinematicFrame::UpdateCached()
{
    mJoints.clear();

    wxSizer* sizer = mCtrScrollWnd->GetSizer();
    if (sizer == 0)
        {
            assert(false);
            return;
        }

    shared_ptr<SimSpark> spark = wxGetApp().GetSpark();
    if (spark.get() == 0)
        {
            return;
        }

    mParent.Update(spark->GetCore());
    shared_ptr<Node> node = shared_dynamic_cast<Node>(mParent.get());
    if (node.get() == 0)
        {
            return;
        }

    mCtrScrollWnd->Freeze();

    bool delete_windows = true;
    sizer->Clear(delete_windows);

    Leaf::TLeafList joints;
    bool recursive = true;
    node->ListChildrenSupportingClass<Joint>(joints,recursive);

    for (
         Leaf::TLeafList::iterator iter = joints.begin();
         iter != joints.end();
         ++iter
         )
        {
            shared_ptr<Joint> joint = shared_static_cast<Joint>(*iter);

            wxString path = joint->GetFullPath();
            wxStaticText* text = new wxStaticText( mCtrScrollWnd, wxID_ANY, path, wxDefaultPosition, wxDefaultSize, 0 );
            sizer->Add(text, 0, wxALIGN_RIGHT|wxALIGN_CENTER_VERTICAL|wxALL|wxADJUST_MINSIZE, 5);

            wxSlider* slider = new wxSlider( mCtrScrollWnd, wxID_ANY, 0, -179, 179, wxDefaultPosition, wxSize(250,-1),
                                             wxSL_HORIZONTAL|wxSL_AUTOTICKS|wxSL_LABELS );
            sizer->Add(slider, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5);

            JointControl entry;
            entry.type = joint->GetType();
            entry.axis = 0;
            entry.joint = joint;

            mJoints[slider] = entry;
        }

    mCtrScrollWnd->Thaw();
    mCtrScrollWnd->Layout();
}

void kinematicFrame::SetParent(weak_ptr<Leaf> parent)
{
    shared_ptr<SimSpark> spark = wxGetApp().GetSpark();
    if (spark.get() == 0)
        {
            return;
        }

    if (parent.expired())
    {
        mParent = Leaf::CachedPath<zeitgeist::Leaf>();
    } else
    {
        std::string path = parent.lock()->GetFullPath();
        mParent.SetKey(Core::CacheKey(spark->GetCore()->GetRoot(), path));
    }

    UpdateCached();
}

void kinematicFrame::RefreshProperties()
{
    for (
         TJointControlMap::iterator iter = mJoints.begin();
         iter != mJoints.end();
         ++iter
         )
        {
            wxSlider* slider = (*iter).first;
            JointControl& entry = (*iter).second;

            if (
                (entry.joint.expired()) ||
                (slider == 0)
                )
                {
                    continue;
                }

            if (entry.mode == CM_ACTIVE)
            {
                MoveJoint(entry);
            }

            int value = 0;

            switch (entry.type)
                {
                default:
                    break;

                case dJointTypeSlider:
                    {
                        shared_ptr<SliderJoint> sliderJoint
                            = shared_static_cast<SliderJoint>(entry.joint.lock());
                        value = static_cast<int>(sliderJoint->GetPosition());
                        break;
                    }

                case dJointTypeUniversal:
                    {
                        shared_ptr<UniversalJoint> universal
                            = shared_static_cast<UniversalJoint>(entry.joint.lock());
                        value = static_cast<int>(universal->GetAngle(static_cast<Joint::EAxisIndex>(entry.axis)));
                        break;
                    }

                case dJointTypeHinge:
                    {
                        shared_ptr<HingeJoint> hinge
                            = shared_static_cast<HingeJoint>(entry.joint.lock());
                        value = static_cast<int>(hinge->GetAngle());
                        break;
                    }

                }

            slider->SetValue(value);
        }
}

void kinematicFrame::OnScrollChanged(wxScrollEvent& event)
{
    TJointControlMap::iterator iter =
        mJoints.find(static_cast<wxSlider*>(event.GetEventObject()));

    if (iter == mJoints.end())
        {
            assert(false);
            return;
        }

    JointControl& entry = (*iter).second;
    shared_ptr<Joint> joint(shared_static_cast<Joint>(entry.joint.lock()));
    joint->SetParameter(dParamVel, 0);
}

float kinematicFrame::GetJointVel(float diff)
{
    float vel = diff;
    return gClamp<float>(vel, -MAX_JOINT_VEL, MAX_JOINT_VEL);
}

void kinematicFrame::MoveJoint(JointControl& entry)
{
    switch (entry.type)
        {
        default:
            break;

        case dJointTypeHinge:
            {
                shared_ptr<HingeJoint> hinge
                    = shared_static_cast<HingeJoint>(entry.joint.lock());
                float value_joint = static_cast<int>(hinge->GetAngle());
                float diff = (entry.target - value_joint);
                if (gAbs(diff) < MIN_JOINT_DIFF)
                {
                    entry.mode = CM_PASSIV;
                    hinge->SetParameter(dParamVel, 0);
                } else
                {
                    hinge->SetParameter(dParamVel, GetJointVel(diff));
                }
                break;
            }

        case dJointTypeUniversal:
            {
                shared_ptr<UniversalJoint> universal
                    = shared_static_cast<UniversalJoint>(entry.joint.lock());
                int value_joint = static_cast<int>(universal->GetAngle(static_cast<Joint::EAxisIndex>(entry.axis)));
                int diff = (entry.target - value_joint);
                if (abs(diff) < MIN_JOINT_DIFF)
                {
                    entry.mode = CM_PASSIV;
                    universal->SetParameter(dParamVel, 0);
                } else
                {
                    universal->SetParameter(dParamVel, GetJointVel(diff));
                }
                break;
            }
        }
}

void kinematicFrame::OnSliderChanged(wxCommandEvent& event)
{
    TJointControlMap::iterator iter =
        mJoints.find(static_cast<wxSlider*>(event.GetEventObject()));

    if (iter == mJoints.end())
        {
            assert(false);
            return;
        }


    wxSlider* slider = (*iter).first;
    JointControl& entry = (*iter).second;

    entry.mode = CM_ACTIVE;
    entry.target = slider->GetValue();
}