source: flair-src/trunk/lib/FlairCore/src/Vector3D.cpp@ 447

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2013/05/02
//  filename:   Vector3D.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class defining a 3D vector
//
//
/*********************************************************************/

#include "Vector3D.h"
#include "Vector2D.h"
#include "RotationMatrix.h"
#include "Euler.h"
#include "Quaternion.h"
#include "Object.h"
#include <math.h>

namespace flair {
namespace core {
  
template class Vector3D<double>;
template Vector3D<double> operator+(const Vector3D<double>&, const Vector3D<double>&);
template Vector3D<double> operator-(const Vector3D<double>&, const Vector3D<double>&);
template Vector3D<double> operator-(const Vector3D<double>&);
template Vector3D<double> operator/(const Vector3D<double>&, float);
template Vector3D<double> operator*(const Vector3D<double>&, const Vector3D<double>&);
template Vector3D<double> operator*(const Vector3D<double>&, float);
template Vector3D<double> operator*(float, const Vector3D<double>&);
template Vector3D<double> CrossProduct(const Vector3D<double>&, const Vector3D<double>&);
template float DotProduct(const Vector3D<double>&, const Vector3D<double>&);

template class Vector3D<float>;
template Vector3D<float> operator+(const Vector3D<float>&, const Vector3D<float>&);
template Vector3D<float> operator-(const Vector3D<float>&, const Vector3D<float>&);
template Vector3D<float> operator-(const Vector3D<float>&);
template Vector3D<float> operator/(const Vector3D<float>&, float);
template Vector3D<float> operator*(const Vector3D<float>&, const Vector3D<float>&);
template Vector3D<float> operator*(const Vector3D<float>&, float);
template Vector3D<float> operator*(float, const Vector3D<float>&);
template Vector3D<float> CrossProduct(const Vector3D<float>&, const Vector3D<float>&);
template float DotProduct(const Vector3D<float>&, const Vector3D<float>&);

template <typename T> Vector3D<T>::Vector3D(T inX, T inY, T inZ) {
  x=inX;
  y=inY;
  z=inZ;
}

template <typename T> Vector3D<T>::~Vector3D() {}

/*
void Vector3D::operator=(const gui::Vector3DSpinBox *vector) {
    Vector3D vect=vector->Value();
    x=vect.x;
    y=vect.y;
    z=vect.z;
}*/
/*
template<typename T,typename S> Vector3D<T> &Vector3D<T>::operator=(const Vector3D<S> &vector) {
//template <typename T> Vector3D<T> &Vector3D<T>::operator=(const Vector3D<T> &vector) {
  x = vector.x;
  y = vector.y;
  z = vector.z;
  return (*this);
}
*/
template <typename T> Vector3D<T> &Vector3D<T>::operator+=(const Vector3D<T> &vector) {
  x += vector.x;
  y += vector.y;
  z += vector.z;
  return (*this);
}

template <typename T> Vector3D<T> &Vector3D<T>::operator-=(const Vector3D<T> &vector) {
  x -= vector.x;
  y -= vector.y;
  z -= vector.z;
  return (*this);
}

template <typename T> T &Vector3D<T>::operator[](size_t idx) {
  if (idx == 0) {
    return x;
  } else if (idx == 1) {
    return y;
  } else if (idx == 2) {
    return z;
  } else {
    Printf("Vector3D: index %i out of bound\n", idx);
    return z;
  }
}

template <typename T> const T &Vector3D<T>::operator[](size_t idx) const {
  if (idx == 0) {
    return x;
  } else if (idx == 1) {
    return y;
  } else if (idx == 2) {
    return z;
  } else {
    Printf("Vector3D: index %i out of bound\n", idx);
    return z;
  }
}

template <typename T> Vector3D<T> CrossProduct(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
  return Vector3D<T>(vectorA.y * vectorB.z - vectorA.z * vectorB.y,
                  vectorA.z * vectorB.x - vectorA.x * vectorB.z,
                  vectorA.x * vectorB.y - vectorA.y * vectorB.x);
}

template <typename T> float DotProduct(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
  return vectorA.x * vectorB.x + vectorA.y * vectorB.y + vectorA.z * vectorB.z;
}

template <typename T> Vector3D<T> operator+(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
  return Vector3D<T>(vectorA.x + vectorB.x, vectorA.y + vectorB.y,
                  vectorA.z + vectorB.z);
}

template <typename T> Vector3D<T> operator-(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
  return Vector3D<T>(vectorA.x - vectorB.x, vectorA.y - vectorB.y,
                  vectorA.z - vectorB.z);
}

template <typename T> Vector3D<T> operator-(const Vector3D<T> &vector) {
  return Vector3D<T>(-vector.x, -vector.y, -vector.z);
}

template <typename T> Vector3D<T> operator*(const Vector3D<T> &vectorA, const Vector3D<T> &vectorB) {
  return Vector3D<T>(vectorA.x * vectorB.x, vectorA.y * vectorB.y,
                  vectorA.z * vectorB.z);
}

template <typename T> Vector3D<T> operator*(const Vector3D<T> &vector, float coeff) {
  return Vector3D<T>(vector.x * coeff, vector.y * coeff, vector.z * coeff);
}

template <typename T> Vector3D<T> operator*(float coeff, const Vector3D<T> &vector) {
  return Vector3D<T>(vector.x * coeff, vector.y * coeff, vector.z * coeff);
}

template <typename T> Vector3D<T> operator/(const Vector3D<T> &vector, float coeff) {
  if (coeff != 0) {
    return Vector3D<T>(vector.x / coeff, vector.y / coeff, vector.z / coeff);
  } else {
    printf("Vector3D: err divinding by 0\n");
    return Vector3D<T>(0, 0, 0);
  }
}

template <typename T> void Vector3D<T>::RotateX(float value) {
  float y_tmp;
  y_tmp = y * cosf(value) - z * sinf(value);
  z = y * sinf(value) + z * cosf(value);
  y = y_tmp;
}

template <typename T> void Vector3D<T>::RotateXDeg(float value) { RotateX(Euler::ToRadian(value)); }

template <typename T> void Vector3D<T>::RotateY(float value) {
  float x_tmp;
  x_tmp = x * cosf(value) + z * sinf(value);
  z = -x * sinf(value) + z * cosf(value);
  x = x_tmp;
}

template <typename T> void Vector3D<T>::RotateYDeg(float value) { RotateY(Euler::ToRadian(value)); }

template <typename T> void Vector3D<T>::RotateZ(float value) {
  float x_tmp;
  x_tmp = x * cosf(value) - y * sinf(value);
  y = x * sinf(value) + y * cosf(value);
  x = x_tmp;
}

template <typename T> void Vector3D<T>::RotateZDeg(float value) { RotateZ(Euler::ToRadian(value)); }

template <typename T> void Vector3D<T>::Rotate(const RotationMatrix &matrix) {
  T a[3] = {0, 0, 0};
  T b[3] = {x, y, z};

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      a[i] += matrix.m[i][j] * b[j];
    }
  }

  x = a[0];
  y = a[1];
  z = a[2];
}

template <typename T> void Vector3D<T>::Rotate(const Quaternion &quaternion) {
  RotationMatrix matrix;
  quaternion.ToRotationMatrix(matrix);
  Rotate(matrix);
}

template <typename T> void Vector3D<T>::To2Dxy(Vector2D<T> &vector) const {
  vector.x = x;
  vector.y = y;
}

template <typename T> Vector2D<T> Vector3D<T>::To2Dxy(void) const {
  Vector2D<T> vect;
  To2Dxy(vect);
  return vect;
}

template <typename T> float Vector3D<T>::GetNorm(void) const { return sqrtf(x * x + y * y + z * z); }

template <typename T> void Vector3D<T>::Normalize(void) {
  float n = GetNorm();
  if (n != 0) {
    x = x / n;
    y = y / n;
    z = z / n;
  }
}

template <typename T> void Vector3D<T>::Saturate(const Vector3D<T> &min, const Vector3D<T> &max) {
  if (x < min.x)
    x = min.x;
  if (x > max.x)
    x = max.x;

  if (y < min.y)
    y = min.y;
  if (y > max.y)
    y = max.y;

  if (z < min.z)
    z = min.z;
  if (z > max.z)
    z = max.z;
}

template <typename T> void Vector3D<T>::Saturate(float min, float max) {
  Saturate(Vector3D<T>(min, min, min), Vector3D<T>(max, max, max));
}

template <typename T> void Vector3D<T>::Saturate(const Vector3D<T> &value) {
  float x = fabs(value.x);
  float y = fabs(value.y);
  float z = fabs(value.z);
  Saturate(Vector3D<T>(-x, -y, -z), Vector3D<T>(x, y, z));
}

template <typename T> void Vector3D<T>::Saturate(float value) {
  float sat = fabs(value);
  Saturate(Vector3D<T>(-sat, -sat, -sat), Vector3D<T>(sat, sat, sat));
}

} // end namespace core
} // end namespace flair