MABodyTypes.cpp

/*
 *  This file is part of the AiBO+ project
 *
 *  Copyright (C) 2005-2016 Csaba Kertész (csaba.kertesz@gmail.com)
 *
 *  AiBO+ 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; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  AiBO+ 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., 59 Temple Street #330, Boston, MA 02111-1307, USA.
 *
 */

#include "MABodyTypes.hpp"

#include "MARobotState.hpp"
#include "MABodyMotion.hpp"
#include "MATypeRanges.hpp"

#include <boost/bind.hpp>

#include <math.h>

MABodyPartFeature::MABodyPartFeature(int value, int minimum, int maximum) :
  Value(value, minimum, maximum)
{
}


MALegPosition::MALegPosition() : MABodyPartFeature(), ForeBackValue(0, -100, 100),
  UpDownValue(0, -100, 100)
{
  MA_SIGNAL_CONNECT(ForeBackValue.GetValueChangeSignal(), MALegPosition::NotifyNumUpdate);
  MA_SIGNAL_CONNECT(UpDownValue.GetValueChangeSignal(), MALegPosition::NotifyNumUpdate);
}


MALegPosition::MALegPosition(const MALegPosition& other) : MABodyPartFeature(),
  ForeBackValue(other.ForeBackValue), UpDownValue(other.UpDownValue)
{
  MA_SIGNAL_CONNECT(ForeBackValue.GetValueChangeSignal(), MALegPosition::NotifyNumUpdate);
  MA_SIGNAL_CONNECT(UpDownValue.GetValueChangeSignal(), MALegPosition::NotifyNumUpdate);
}


void MALegPosition::FromJointDegree(int degree)
{
  ForeBackValue.SetBlockedNotifications(true);
  ForeBackValue = DegreeToForeBack(degree);
  ForeBackValue.SetBlockedNotifications(false);
  UpDownValue.SetBlockedNotifications(true);
  UpDownValue = DegreeToUpDown(degree);
  UpDownValue.SetBlockedNotifications(false);
}


int MALegPosition::ToJointDegree() const
{
  float ForeBack = (float)ForeBackValue;
  float UpDown = (float)UpDownValue;
  int Ret = 0;

  if (UpDown < 0)
  {
    Ret = 90+(int)round(UpDown*90 / 100);
    if (ForeBack < 0)
      Ret = -Ret;
    return Ret;
  }
  if (ForeBack < 0)
    Ret = (int)-(round(UpDown*(-MA::Joint1Min-90) / 100)+90);
  else
    Ret = (int)(round(UpDown*(MA::Joint1Max-90) / 100)+90);
  return Ret;
}


void MALegPosition::NotifyNumUpdate(MANum<float>& number)
{
  if (&number == &UpDownValue)
  {
    int Degree = ToJointDegree();

    ForeBackValue.SetBlockedNotifications(true);
    ForeBackValue = DegreeToForeBack(Degree);
    ForeBackValue.SetBlockedNotifications(false);
  }
  if (&number == &ForeBackValue)
  {
    int Degree = (int)round((float)ForeBackValue*90 / 100);;

    UpDownValue.SetBlockedNotifications(true);
    UpDownValue = DegreeToUpDown(Degree);
    UpDownValue.SetBlockedNotifications(false);
  }
}


float MALegPosition::DegreeToForeBack(int degree) const
{
  float Ret = 0.0;

  if (unlikely(degree >= 90))
    Ret = 100.0;
  else
  if (unlikely(degree <= -90))
    Ret = -100.0;
  else
    Ret = (float)(degree*100) / 90;
  return Ret;
}


float MALegPosition::DegreeToUpDown(int degree) const
{
  float Ret = 0.0;

  if (likely(MCAbs(degree) <= 90))
  {
    Ret = -(float)((90-MCAbs(degree))*100) / 90;
  }
  else
  if (unlikely(degree < -90))
    Ret = -(float)((degree+90)*100) / (-MA::Joint1Min-90);
  else
  if (unlikely(degree > 90))
    Ret = (float)((degree-90)*100) / (MA::Joint1Max-90);
  return Ret;
}


MASidelong::MASidelong(int value) : MABodyPartFeature(value, 0, 100)
{
}


void MASidelong::FromJointDegree(int degree)
{
  Value = (float)((degree-MA::Joint2Min)*100) / (-MA::Joint2Min+MA::Joint2Max);
}


int MASidelong::ToJointDegree() const
{
  return (int)round((float)Value*(-MA::Joint2Min+MA::Joint2Max) / 100)+MA::Joint2Min;
}


MABentStretched::MABentStretched(int value) : MABodyPartFeature(value, 0, 100)
{
}


void MABentStretched::FromJointDegree(int degree)
{
  Value = degree <= 0 ? 0 : (float)(degree*100) / MA::Joint3Max;
}


int MABentStretched::ToJointDegree() const
{
  return (int)round((float)Value*MA::Joint3Max / 100);
}


MALeftRightTurned::MALeftRightTurned(int value) : MABodyPartFeature(value)
{
}


void MALeftRightTurned::FromJointDegree(int degree)
{
  Value = -(float)(degree*100) / MA::NeckPanMax;
}


int MALeftRightTurned::ToJointDegree() const
{
  return -(int)round((float)Value*MA::NeckPanMax / 100);
}


MAHeadPosition::MAHeadPosition() : ForeBackValue(0, -100, 100), UpDownValue(0, -100, 100)
{
}


void MAHeadPosition::FromJointDegree(int degree)
{
  MC_UNUSED(degree)
  // Nothing to do
}


int MAHeadPosition::ToJointDegree() const
{
  // Nothing to do
  return 0;
}


void MAHeadPosition::FromJointDegrees(int neck_tilt1, int neck_tilt2)
{
  MANum<int> NeckTilt1(neck_tilt1, MA::NeckT1Min, MA::NeckT1Max);
  MANum<int> NeckTilt2(neck_tilt2, MA::NeckT2Min, MA::NeckT2Max);
  int ZeroDegreeCorrection = (MA::RobotState.get() && MA::RobotState->BodyMotion->Sitting > 80) ? 35 : 0;
  int ZeroDegree = -NeckTilt1-ZeroDegreeCorrection;

  ForeBackValue = -(float)((int)NeckTilt1+ZeroDegreeCorrection);
  UpDownValue = (float)((int)NeckTilt2-ZeroDegree);
}


int MAHeadPosition::ToNeckTilt1Degree() const
{
  int ZeroDegreeCorrection = (MA::RobotState.get() && MA::RobotState->BodyMotion->Sitting > 80) ? 35 : 0;
  MANum<int> Degree(-((int)ForeBackValue+ZeroDegreeCorrection), MA::NeckT1Min, MA::NeckT1Max);

  return (int)Degree;
}


int MAHeadPosition::ToNeckTilt2Degree() const
{
  int ZeroDegreeCorrection = (MA::RobotState.get() && MA::RobotState->BodyMotion->Sitting > 80) ? 35 : 0;
  MANum<int> Degree((int)UpDownValue-ToNeckTilt1Degree()-ZeroDegreeCorrection, MA::NeckT2Min, MA::NeckT2Max);

  return (int)Degree;
}


MAMouthOpened::MAMouthOpened(int value) : MABodyPartFeature(value, 0, 100)
{
}


void MAMouthOpened::FromJointDegree(int degree)
{
  Value = degree <= -60.0 ? 100.0 : 100.0-(float)((degree-MA::MouthMin)*100) / (MA::MouthMax-MA::MouthMin);
}


int MAMouthOpened::ToJointDegree() const
{
  return MA::MouthMin+(int)round((float)(100.0-Value)*(MA::MouthMax-MA::MouthMin) / 100);
}