/*
  Sss - a slope soaring simulater.
  Copyright (C) 2002 Danny Chapman - flight@rowlhouse.freeserve.co.uk
*/
#ifndef MATRIX_VECTOR3
#define MATRIX_VECTOR3

#include <math.h>
#include "precision.hpp"

typedef unsigned int uint;

class Vector3;

//! A 3x3 matrix
class Matrix3
{
public:
  inline Matrix3(Scalar v11, Scalar v21, Scalar v31, // first column
                 Scalar v12, Scalar v22, Scalar v32, // 2nd column
                 Scalar v13, Scalar v23, Scalar v33  );
  inline Matrix3(const Vector3 & v1, // first column
                 const Vector3 & v2, 
                 const Vector3 & v3);
  inline Matrix3();
  explicit inline Matrix3(Scalar val);
  inline ~Matrix3();

  inline void set_to(Scalar val);
  inline void orthonormalise();
  
  inline bool sensible() const; // indicates if all is OK

  Scalar & operator()(const uint i, const uint j) {return data[i + 3*j];}
  const Scalar & operator()(const uint i, const uint j) const {return data[i + 3*j];}
  
  //! returns pointer to the first element
  inline const Scalar * get_data() {return data;} 
  inline const Scalar * get_data() const {return data;} 
  //! pointer to value returned from get_data
  inline void set_data(const Scalar * d); 
  
  /// Returns a column - no range checking!
  inline Vector3 get_col(uint i) const;

  // sets a column
  inline void set_col(uint i, const Vector3 & col);

  // operators
  inline Matrix3 & operator+=(const Matrix3 & rhs);
  inline Matrix3 & operator-=(const Matrix3 & rhs);

  inline Matrix3 & operator*=(const Scalar rhs);
  inline Matrix3 & operator/=(const Scalar rhs);

  inline Matrix3 operator+(const Matrix3 & rhs) const;
  inline Matrix3 operator-(const Matrix3 & rhs) const;
  friend Matrix3 operator*(const Matrix3 & lhs, const Scalar rhs);
  friend Matrix3 operator*(const Scalar lhs, const Matrix3 & rhs);
  friend Matrix3 operator*(const Matrix3 & lhs, const Matrix3 & rhs);
  friend Matrix3 transpose(const Matrix3 & rhs);
  friend Scalar trace(const Matrix3 & rhs);  
  friend Vector3 operator*(const Matrix3 & lhs, const Vector3 & rhs);

  inline void show(const char * str = "") const;
  
private:
  Scalar data[9];
};

//############## Vector3 ################
//! A 3x1 matrix (i.e. a vector)
class Vector3
{
public:
  inline Vector3() {};
  explicit inline Vector3(Scalar val);
  inline Vector3(Scalar x, Scalar y, Scalar z) 
    {data[0] = x; data[1] = y; data[2] = z;}
  inline ~Vector3() {};
  
  inline void set_to(Scalar val); //!< Set all values to val
  
  inline bool sensible() const; // indicates if all is OK

  Scalar & operator[](uint i) {return data[i];} //!< unchecked access
  const Scalar & operator[](uint i) const {return data[i];}
  Scalar & operator()(uint i) {return data[i];}
  const Scalar & operator()(uint i) const {return data[i];}
  
  //! returns pointer to the first element
  inline const Scalar * get_data() {return data;} 
  inline const Scalar * get_data() const {return data;} 
  //! pointer to value returned from get_data
  inline void set_data(const Scalar * d);
  
  //! calculate the square of the magnitude
  inline Scalar mag2() const {
    return (Scalar) (data[0]*data[0]+data[1]*data[1]+data[2]*data[2]);}
  //! calculate the magnitude
  inline Scalar mag() const {return (Scalar) sqrt(mag2());}
  //! Normalise, and return the result
  inline Vector3 & normalise();
  
  // operators
  inline Vector3 & operator+=(const Vector3 & rhs);
  inline Vector3 & operator-=(const Vector3 & rhs);

  inline Vector3 & operator*=(const Scalar rhs);
  inline Vector3 & operator/=(const Scalar rhs);

  inline Vector3 operator-() const {return Vector3(-data[0], -data[1], -data[2]);}
  
  inline Vector3 operator+(const Vector3 & rhs) const;
  inline Vector3 operator-(const Vector3 & rhs) const;

  friend Vector3 operator*(const Vector3 & lhs, const Scalar rhs);
  friend Vector3 operator*(const Scalar lhs, const Vector3 & rhs);
  friend Vector3 operator/(const Vector3 & lhs, const Scalar rhs);
  friend Scalar dot(const Vector3 & lhs, const Vector3 & rhs);
  friend Vector3 cross(const Vector3 & lhs, const Vector3 & rhs);
  
  friend Vector3 operator*(const Matrix3 & lhs, const Vector3 & rhs);
  
  friend Matrix3 rotation_matrix(Scalar ang, const Vector3 & dir);
  
  inline void show(const char * str = "") const;
  
private:
  Scalar data[3];
};

// global operators

inline Matrix3 operator*(const Matrix3 & lhs, const Scalar rhs);
inline Matrix3 operator*(const Scalar lhs, const Matrix3 & rhs) {return rhs * lhs;}
inline Matrix3 operator*(const Matrix3 & lhs, const Matrix3 & rhs);

inline Vector3 operator*(const Vector3 & lhs, const Scalar rhs);
inline Vector3 operator/(const Vector3 & lhs, const Scalar rhs);
inline Scalar dot(const Vector3 & lhs, const Vector3 & rhs);
inline Vector3 cross(const Vector3 & lhs, const Vector3 & rhs);
inline Vector3 operator*(const Scalar lhs, const Vector3 & rhs) {return rhs * lhs;}
inline Matrix3 transpose(const Matrix3 & rhs);

inline Scalar trace(const Matrix3 & rhs);

// matrix * vector
inline Vector3 operator*(const Matrix3 & lhs, const Vector3 & rhs);

// Some useful rotation Matrix3's
// alpha returns a matrix that wil rotate alpha around the x axis (etc)
inline Matrix3 m3alpha(Scalar alpha);
inline Matrix3 m3beta(Scalar beta);
inline Matrix3 m3gamma(Scalar gamma);

inline const Matrix3 & matrix3_identity()
{
  static const Matrix3 result(1, 0, 0,
                              0, 1, 0,
                              0, 0, 1);
  return result;
}

inline Matrix3 rotation_matrix(Scalar ang, const Vector3 & dir);

// converts a rotation matrix into a rotation of degrees about axis
inline void calculate_rot_from_matrix(const Matrix3 & matrix, Vector3 & axis, Scalar & degrees);


#include "matrix_vector3.inl"

#endif // include file



