typeUtils.cpp

/*
  型定義
  Saotufmi KAMIMURA
  $Id$
*/

#include "typeUtils.h"


const VXV::Position& VXV::Position::operator+=(const Position& rhs) {
  x += rhs.x;
  y += rhs.y;
  zt += rhs.zt;
  return *this;
}


const VXV::Position& VXV::Position::operator-=(const Position& rhs) {
  x -= rhs.x;
  y -= rhs.y;
  zt -= rhs.zt;
  return *this;
}


int VXV::length(const Grid3D& position) {
  return static_cast<int>(sqrt(1.0 * ((position.x * position.x) +
                                      (position.y * position.y) +
                                      (position.z * position.z))));
}


const VXV::Grid& VXV::Grid::operator+=(const Grid& rhs) {
  x += rhs.x;
  y += rhs.y;
  return *this;
}


const VXV::Grid& VXV::Grid::operator-=(const Grid& rhs) {
  x -= rhs.x;
  y -= rhs.y;
  return *this;
}


const VXV::Grid3D& VXV::Grid3D::operator+=(const Grid3D& rhs) {
  x += rhs.x;
  y += rhs.y;
  z += rhs.z;
  return *this;
}


const VXV::Grid3D& VXV::Grid3D::operator-=(const Grid3D& rhs) {
  x -= rhs.x;
  y -= rhs.y;
  z -= rhs.z;
  return *this;
}


const int VXV::Direction::to_deg(void) const {
  return (int)rint(360.0 * to_rad() / (2.0 * M_PI));
}


const double VXV::Direction::to_rad(void) const {
  int times = static_cast<int>(inner_radian / (2.0 * M_PI));
  double radian = (inner_radian - (2.0 * M_PI * times));
  radian = (radian < 0) ? (2.0 * M_PI + radian) : radian;

  return (radian > M_PI) ? (radian - 2.0 * M_PI) : radian;
}


const VXV::Position3D& VXV::Position3D::operator+=(const Position3D& rhs) {
  x += rhs.x;
  y += rhs.y;
  z += rhs.z;
  xt += rhs.xt;
  yt += rhs.yt;
  zt += rhs.zt;
  return *this;
}


const VXV::Position3D& VXV::Position3D::operator-=(const Position3D& rhs) {
  x -= rhs.x;
  y -= rhs.y;
  z -= rhs.z;
  xt -= rhs.xt;
  yt -= rhs.yt;
  zt -= rhs.zt;
  return *this;
}


VXV::Matrix4D VXV::createConvertMatrix(const Position3D pos) {
  Matrix4D conv;

  conv.a11 = cos(pos.yt.to_rad()) * cos(pos.zt.to_rad());
  conv.a21 = -cos(pos.xt.to_rad())*sin(pos.zt.to_rad())
    + sin(pos.xt.to_rad())*sin(pos.yt.to_rad())*cos(pos.zt.to_rad());
  conv.a31 = cos(pos.xt.to_rad())*sin(pos.yt.to_rad())*cos(pos.zt.to_rad())
    + sin(pos.xt.to_rad())*sin(pos.zt.to_rad());
  conv.a41 = pos.x;

  conv.a12 = cos(pos.yt.to_rad())*sin(pos.zt.to_rad());
  conv.a22 = cos(pos.xt.to_rad())*cos(pos.zt.to_rad())
    + sin(pos.xt.to_rad())*sin(pos.yt.to_rad())*sin(pos.zt.to_rad());
  conv.a32 = cos(pos.xt.to_rad())*sin(pos.yt.to_rad())*sin(pos.zt.to_rad())
    - sin(pos.xt.to_rad())*cos(pos.zt.to_rad());
  conv.a42 = pos.y;

  conv.a13 = -sin(pos.yt.to_rad());
  conv.a23 = sin(pos.xt.to_rad())*cos(pos.yt.to_rad());
  conv.a33 = cos(pos.xt.to_rad())*cos(pos.yt.to_rad());
  conv.a43 = pos.z;

  return conv;
}


void VXV::convert(Position3D& converted, const Position3D& input,
                  const Matrix4D& matrix) {
  Position3D pos;
  pos.x = (int)(input.x * matrix.a11 +
                      input.y * matrix.a21 +
                      input.z * matrix.a31) + matrix.a41;
  pos.y = (int)(input.x * matrix.a12 +
                      input.y * matrix.a22 +
                      input.z * matrix.a32) + matrix.a42;
  pos.z = (int)(input.x * matrix.a13 +
                      input.y * matrix.a23 +
                      input.z * matrix.a33) + matrix.a43;
  converted.x = pos.x;
  converted.y = pos.y;
  converted.z = pos.z;
}


void VXV::convertWithAngle(Position3D& converted, const Position3D& input,
                           const Position3D& forMatrix) {

  Matrix4D convert = createConvertMatrix(forMatrix);
  VXV::convert(converted, input, convert);
  converted.xt += forMatrix.xt;
  converted.yt += forMatrix.yt;
  converted.zt += forMatrix.zt;
}