kinematicframe.cpp

rcssserver3d Robocup 3D比赛官方指定平台

/* -*- 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 2008/02/24 16:20:22 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>
#include <rsgedit/res/xpm_universal.xpm>
#include <rsgedit/res/xpm_slider.xpm>
#include <rsgedit/res/xpm_hinge.xpm>

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),
    mBmpUniversal(xpm_universal), mBmpSlider(xpm_slider), mBmpHinge(xpm_hinge)
{
    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 = 3;
    int vgap = 0;
    int hgap = 0;

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

kinematicFrame::~kinematicFrame()
{
}

void kinematicFrame::AddJointDescription(shared_ptr<Joint> joint)
{
    wxSizer* sizer = mCtrScrollWnd->GetSizer();
    if (
        (sizer == 0) ||
        (joint.get() == 0)
        )
        {
            assert(false);
            return;
        }

    sizer->AddSpacer(0);

    wxStaticBitmap* bmp = 0;
    wxString strType;

    const int jt = joint->GetType();
    switch (jt)
        {
        default:
            assert(false);
            strType = wxT("?");
            break;

        case dJointTypeUniversal:
            bmp = new wxStaticBitmap(mCtrScrollWnd, wxID_ANY, mBmpUniversal);
            strType = wxT("Universal");
            break;

        case dJointTypeHinge:
            bmp = new wxStaticBitmap(mCtrScrollWnd, wxID_ANY, mBmpHinge);
            strType = wxT("Hinge");
            break;

        case dJointTypeSlider:
            bmp = new wxStaticBitmap(mCtrScrollWnd, wxID_ANY, mBmpSlider);
            strType = wxT("Slider");
            break;
        }

    if (bmp == 0)
        {
            sizer->AddSpacer(0);
        } else
        {
            sizer->Add(bmp, 0, wxALIGN_LEFT|wxALIGN_CENTER_VERTICAL|wxALL|wxADJUST_MINSIZE, 5);
        }

    wxString path = strType + wxT(" at ") + wxString::FromAscii(joint->GetFullPath().c_str());
    wxStaticText* text =
        new wxStaticText(mCtrScrollWnd, wxID_ANY, path, wxDefaultPosition, wxDefaultSize, 0 );

    sizer->Add(text, 0, wxALIGN_LEFT|wxALIGN_CENTER_VERTICAL|wxALL|wxADJUST_MINSIZE, 5);
}

void kinematicFrame::AddJointControl(shared_ptr<Joint> joint, int axis)
{
    wxSizer* sizer = mCtrScrollWnd->GetSizer();
    if (
        (sizer == 0) ||
        (joint.get() == 0)
        )
        {
            assert(false);
            return;
        }


    sizer->AddSpacer(0);

    wxString label(wxString::Format(wxT("%d :"),axis));
    wxStaticText* text =
        new wxStaticText( mCtrScrollWnd, wxID_ANY, label, 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_LABELS );
    sizer->Add(slider, 0, wxGROW|wxALIGN_CENTER_VERTICAL|wxALL, 5);

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

    mJoints[slider] = entry;
}

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);
            int jt = joint->GetType();

            switch (jt)
                {
                case dJointTypeFixed:
                    break;

                case dJointTypeUniversal:
                    AddJointDescription(joint);
                    AddJointControl(joint, 0);
                    AddJointControl(joint, 1);
                    break;

                default:
                    AddJointDescription(joint);
                    AddJointControl(joint, 0);
                    break;
                }
        }

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

int kinematicFrame::GetParamVel(int axis) const
{
    return (axis == 0) ? dParamVel : dParamVel2;
}

void kinematicFrame::MoveJoint(JointControl& entry)
{
    int paramVel = GetParamVel(entry.axis);

    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(paramVel, 0);
                } else
                {
                    hinge->SetParameter(paramVel, 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(paramVel, 0);
                } else
                {
                    universal->SetParameter(paramVel, 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();
}