//
// Created by zx on 22-12-1.
//
//
// Created by zx on 2020/9/9.
//

#include "pose2d.h"
#include <math.h>
Pose2d::Pose2d()
        :m_x(0),m_y(0),m_theta(0)
{
}
Pose2d::Pose2d(float x,float y,float theta)
        :m_x(x),m_y(y),m_theta(theta)
{
}

Pose2d::~Pose2d()
{}

std::ostream& operator<<(std::ostream &out,const Pose2d& point)
{
  out<<"[x:"<<point.x()<<", y:"<<point.y()<<", theta:"<<point.theta()*180.0/M_PI<<"°]";
  return out;
}
const float Pose2d::gridient() const
{
  double gradient=tanf(m_theta);
  if(fabs(gradient)>200)
    return 200.0*(gradient/fabs(gradient));
  return gradient;
}

Pose2d Pose2d::PoseByPose(const Pose2d& world_pose,const Pose2d& axis_pose)
{
  Pose2d diff=world_pose-axis_pose;
  Pose2d nPose=diff.rotate(-axis_pose.theta());
  return Pose2d(nPose.x(),nPose.y(),diff.theta());
}

float Pose2d::distance(const Pose2d& pose1,const Pose2d& pose2)
{
  Pose2d offset=pose1-pose2;
  return sqrt(offset.x()*offset.x()+offset.y()*offset.y());
}

Pose2d Pose2d::rotate(float theta)const
{
  double cs=cos(theta);
  double sn=sin(theta);
  float x=cs*this->x()-sn*this->y();
  float y=sn*this->x()+cs*this->y();
  float new_theta=this->theta()+theta;
  return Pose2d(x,y,new_theta);
}