///////////////////////////////////////////////////////////////////////////////
//
//      ##  ######          
//       ######  ###        Based on:
//  ## ###############      
//   ########## # # #       Shark 3D Engine (www.shark3d.com)
//    ########
//   ######### # # #        Copyright (c) 1996-2001 Spinor GmbH.
//  ##   ##########         
//      ##                  
//        
///////////////////////////////////////////////////////////////////////////////
//
// G A M E.O.N.E - LOW LEVEL LIB V1.0
// Copyright (C) 2001 LEVEL ONE ENTERTAINMENT, 
// Licensed under the terms of LGPL.
//:---------------------------------------------------------------------------
//:Description
//
// LOW LEVEL 3D VEC3 UTIL HEADERFILE
//
//:---------------------------------------------------------------------------

#ifndef _LL3D_VEC3_H
#define _LL3D_VEC3_H


#include "ll3d_math.h"

namespace ll3d {

template<typename T> struct CVec3;
template<typename T> struct CMat3x3;
template<typename T> struct CQuat;


typedef CVec3<float>	 CVec3f;
typedef const CVec3f	 CVec3f_c;
typedef CVec3f			&CVec3f_r;
typedef const CVec3f	&CVec3f_cr;
typedef CVec3<double>	 CVec3d;
typedef const CVec3d	 CVec3d_c;
typedef CVec3d			&CVec3d_r;
typedef const CVec3d	&CVec3d_cr;

typedef CMat3x3<float>	 CMat3x3f;
typedef const CMat3x3f	 CMat3x3f_c;
typedef CMat3x3f		&CMat3x3f_r;
typedef const CMat3x3f	&CMat3x3f_cr;
typedef CMat3x3<double>  CMat3x3d;
typedef const CMat3x3d	 CMat3x3d_c;
typedef CMat3x3d		&CMat3x3d_r;
typedef const CMat3x3d	&CMat3x3d_cr;

typedef CQuat<float>	 CQuatf;
typedef const CQuatf	 CQuatf_c;
typedef CQuatf			&CQuatf_r;
typedef const CQuatf	&CQuatf_cr;
typedef CQuat<double>	 CQuatd;
typedef const CQuatd	 CQuatd_c;
typedef CQuatd			&CQuatd_r;
typedef const CQuatd	&CQuatd_cr;

/*
 * 3D Vector
 */
template<typename T>
struct CVec3
{
	T m_x, m_y, m_z;

	CVec3<T>();		// Default constructor. Note that the components are not initialized.
	CVec3<T>(T s);	// Set each component to the same value.
	CVec3<T>(T x, T y, T z); // Constructor
	CVec3<T>(const CVec3<T> &v); // Copy constructor
	CVec3<T> &operator=(const CVec3<T> &v); // Assignment operator.
	CVec3<T> operator+() const;
	CVec3<T> operator-() const; // Negation operator.
	CVec3<T> &operator+=(const CVec3<T> &v);
    CVec3<T> &operator-=(const CVec3<T> &v);
    CVec3<T> &operator*=(T s);
    CVec3<T> &operator/=(T s);

    T GetSqLen() const;	// Square of the vector.
    T GetLen() const;	// Square of the vector.

    CVec3<T> GetNormalized() const;			// Normalized to unit length.
    CVec3<T> GetNormalizedTo(T Len) const;	// Normalized to length

    bool IsNull() const;					// Are all components null?
    bool IsFinite() const;					// Are all components finit values?

	// Linear interpolation between two vectors.
	// Returns (*this) + (v - (*this)) * Frac.
    CVec3<T> InterpLinear(const CVec3<T> &v, T Frac) const; 

    // Project v onto Dir. It is 
    // v.GetProjAlong() = Dir * (Vec.GetSqLen() / (Vec * Dir)).
    // It is except for rounding errors 
    // v = v.GetProjOrtho() + v.GetProjAlong().
    CVec3<T> GetProjAlong(const CVec3<T> &Dir) const;

    // Project v onto the plane orthogonal to Dir.
    // It is v.GetProjOrtho() = v - v.GetProjAlong().
    // It is except for rounding errors 
    // v = v.GetProjOrtho() + v.GetProjAlong().
    CVec3<T> GetProjOrtho(const CVec3<T> &Dir) const;

    // Get constraint vector of v onto Dir. It is 
    // v.GetConstrAlong(Dir) = Dir * (v.GetSqLen() / (Vec * Dir)).
    CVec3<T> GetConstrAlong(const CVec3<T> &Dir) const;

    // Get constraint vector v onto the plane 
    // orthogonal to Dir.
    CVec3<T> GetConstrOrtho(const CVec3<T> &Dir) const;

    // Vector divided by its squared length.
    // v.GetReziproc() = v / v.GetSqLen().
    CVec3<T> GetReziproc() const;

    
    CVec3<T> GetAbsComp() const;	// Return for each component the absolute value.
    CVec3<T> GetSignComp() const;	// Return the sign of each component.
    T GetMinComp() const;			// Return the smallest component.
    T GetMaxComp() const;			// Return the largest component.
    T GetSumAbsComp() const;		// Return the sum of the absolute value of all components.
    T GetMaxAbsComp() const;		// Return the maximum of the absolute value of all components.

    CVec3<T> &SetCompToMin(const CVec3<T> &v);	// Set to the minimum component wise.
    CVec3<T> &SetCompToMax(const CVec3<T> &v);	// Set to the maximum component wise.

    CVec3<T> CompMin(const CVec3<T> &v) const;
	CVec3<T> CompMax(const CVec3<T> &v) const;
	CVec3<T> CompMin(const CVec3<T> &v2, const CVec3<T> &v3) const;
    CVec3<T> CompMax(const CVec3<T> &v2, const CVec3<T> &v3) const;

    CVec3<T> operator+(const CVec3<T> &v) const;
    CVec3<T> operator-(const CVec3<T> &v) const;

    
    CVec3<T> operator*(T s) const;	// Scalar multiplication.
    CVec3<T> operator/(T s) const;	// Scalar division.

    T operator*(const CVec3<T> &v) const;	// Scalar product.
    CVec3<T> operator^(const CVec3<T> &v) const; // Cross product of two vectors.

    CVec3<T> operator*(const CMat3x3<T> &m) const;
   
    bool operator==(const CVec3<T> &v) const;	// Equality.
    bool operator!=(const CVec3<T> &v) const;	// Inequality.

    int CompareComp(const CVec3<T> &v) const;	// Defines an order on vectors.

    T Area(const CVec3<T> &v) const		;// Area spanned by v1 AND v2.

    // Volume spanned by v1, v2 and v3.
    // Guaranteed to be null if two vectors are equal.
    T Volume(const CVec3<T> &v2, const CVec3<T> &v3) const;

    
    CVec3<T> CompProd(const CVec3<T> &v) const;	// Component wise product.
    CVec3<T> CompQuot(const CVec3<T> &v) const;	// Component wise division.
    CVec3<T> CompReziproc() const;		// Reziproc of each component.

    
    bool IsCosAngleAbove(const CVec3<T> &v, T c) const;	// Returns true if cos(angle_between(v1, v2)) > c
    bool IsEachCompNe(const CVec3<T> &v) const;
    bool IsEachCompLt(const CVec3<T> &v) const;
    bool IsEachCompLe(const CVec3<T> &v) const;
    bool IsEachCompGt(const CVec3<T> &v) const;
    bool IsEachCompGe(const CVec3<T> &v) const;
    bool IsAnyCompEq(const CVec3<T> &v) const;
    bool IsAnyCompLt(const CVec3<T> &v) const;
    bool IsAnyCompLe(const CVec3<T> &v) const;
    bool IsAnyCompGt(const CVec3<T> &v) const;
    bool IsAnyCompGe(const CVec3<T> &v) const;

    // Scalar multiplication with t, cross product from left with x,y,z.
    // Return [v * q.t, v ^ q.v].
    // Warning! This operator does not rotate and scale the vector 
    // by the scaled rotation representated by the quaternion.
    // To perform this, you must use the matrix 
    // returned by CQuat::GetMat. 
    CVec3<T> operator*(const CQuat<T> &q) const;
};


/*
 * 3x3 Matrix
 */
template<typename T>
struct CMat3x3
{
    T m_xx, m_xy, m_xz, 
      m_yx, m_yy, m_yz, 
      m_zx, m_zy, m_zz;

    CMat3x3<T>();	// Default constructor. Note that the components are not initialized.

    // Initializating with multiple of unity matrix.
    // Also allows to use 0 as null matrix and 1 as unity matrix.
    CMat3x3<T>(T s);

    // Constructor.
    CMat3x3<T>(
        T xx, T xy, T xz, 
        T yx, T yy, T yz, 
        T zx, T zy, T zz);

    
    CMat3x3<T>(const CMat3x3<T> &m);			// Copy constructor.
    CMat3x3<T> &operator=(const CMat3x3<T> &m);	// Assignment operator.

    CMat3x3<T> operator+() const;
    CMat3x3<T> operator-() const;	// Negation operator.
    CMat3x3<T> &operator+=(const CMat3x3<T> &m);
    CMat3x3<T> &operator-=(const CMat3x3<T> &m);
    CMat3x3<T> &operator*=(const CMat3x3<T> &m);
    CMat3x3<T> &operator*=(T s);
    CMat3x3<T> &operator/=(T s);
    
    CMat3x3<T> GetTransposed() const;	// Returns the transposed matrix.

    // Return the transposed inverse matrix 
    // times the determinant of the matrix.
    CMat3x3<T> GetSubDet() const;

    CMat3x3<T> GetInv() const;	// Return the inverse matrix.
    
    T GetSqLen() const;	// Returns the sum of the square if all components.
    T GetLen() const;	// Returns the root of the sum of the square of all components.

    // Returns the determinant.
    // Guaranteed to be null if two rows or colums are equal.
    T GetDet() const;

    
    T GetTrace() const;	// Returns the sum of the diagonal elements.

    // Returns the dual vector of the matrix.
    // The dual vector is the contraction of the levicita tensor 
    // with the matrix interpretated as tensor of second rank.
    // v_a = e_abc M^bc.
    CVec3<T> GetDual() const;

    
    bool IsNull() const;	// Are all components null?
    bool IsFinite() const;	// Are all components finite?

    
    CVec3<T> GetRowX() const;	// Return the vector build of m_xx, m_xy, m_xz.
    CVec3<T> GetRowY() const;	// Return the vector build of m_yx, m_yy, m_yz.
    CVec3<T> GetRowZ() const;	// Return the vector build of m_zx, m_zy, m_zz.
    CVec3<T> GetColX() const;	// Return the vector build of m_xx, m_yx, m_zx.
    CVec3<T> GetColY() const;	// Return the vector build of m_xy, m_yy, m_zy.
    CVec3<T> GetColZ() const;	// Return the vector build of m_xz, m_yz, m_zz.

    void SetRowX(const CVec3<T> &v);
    void SetRowY(const CVec3<T> &v);
    void SetRowZ(const CVec3<T> &v);
    void SetColX(const CVec3<T> &v);
    void SetColY(const CVec3<T> &v);
    void SetColZ(const CVec3<T> &v);

    
    CMat3x3<T> GetAbsComp() const;// Builds the absolute value of each component.
    T GetSumAbsComp() const;// Returns the sum of the absolute values of each component.
    T GetMaxAbsComp() const;// Returns the maximal absolute value of all components.
    CVec3<T> GetSumAbsRows() const;// Returns per the sum of the absolute values of the components of each row.
    CVec3<T> GetSumAbsCols() const;// Returns per the sum of the absolute values of the components of each column.
    CVec3<T> GetMaxAbsRows() const;// Returns per the maximum of the absolute values of the components of each row.
    CVec3<T> GetMaxAbsCols() const;// Returns per the maximum of the absolute values of the components of each column.
    CVec3<T> GetLenRows() const;// Returns the length of the row vectors.
    CVec3<T> GetLenCols() const;// Returns the length of the column vectors.
    CMat3x3<T> GetScaledRows(const CVec3<T> &v) const;// Returns the length of the column vectors.
    CMat3x3<T> GetScaledCols(const CVec3<T> &v) const;// Multiplies each column the the corresponding component of v.

    // Tensor product of two vectors.
    // This is no global function to explicitely exclude type conversions 
    // for the first argument.
    static CMat3x3<T> ByTensorProd(const CVec3<T> &v1, const CVec3<T> &v2);

    // The dual matrix of a vector is the contraction of the levicita tensor 
    // with the vector.
    // M_ab = e_abc v^c.
    static CMat3x3<T> ByDual(const CVec3<T> &v);
    static CMat3x3<T> ByDiag(const CVec3<T> &v);// Create a diagonal matrix.
    static CMat3x3<T> ByDiag(T x, T y, T z);// Create a diagonal matrix.
    static CMat3x3<T> ByRows(
                    const CVec3<T> &vx, 
                    const CVec3<T> &vy, 
                    const CVec3<T> &vz);	// Create a matrix by its row vectors.

    
    static CMat3x3<T> ByCols(
                    const CVec3<T> &vx, 
                    const CVec3<T> &vy, 
                    const CVec3<T> &vz);// Create a matrix by its column vectors.

    CMat3x3<T> operator+(const CMat3x3<T> &m) const;
    CMat3x3<T> operator-(const CMat3x3<T> &m) const;
    CMat3x3<T> operator*(T s) const;// Scalar multiplication.
    CMat3x3<T> operator/(T s) const;
    CMat3x3<T> operator*(const CMat3x3<T> &m) const;// Matrix multiplication.

    CVec3<T> operator*(const CVec3<T> &v) const;

    // Multiply the vector x,y,z with the matrix from left.
    // Return [q.t, m * q.v]
    CQuat<T> operator*(const CQuat<T> &q);

    bool operator==(const CMat3x3<T> &m) const;
    bool operator!=(const CMat3x3<T> &m) const;

    // Defines an order on vectors.
    int CompareComp(const CMat3x3<T> &m) const;
};


/*
 * Quaternion
 *
 * A quaternion has four components (t, x, y, z).
 * It represents a concatication of a rotation and a scaling
 * in the following way.
 * The direction of the vector (x, y, z) is the rotation axis.
 * The length of the vector (x, y, z) is sqrt(scale) * sin(angle/2)
 * and t is sqrt(scale) * cos(angle/2),
 * where angle is the rotation angle, and scale is the scaling factor.
 *
 */

template<typename T>
struct CQuat
{
    T m_t, m_x, m_y, m_z;

    // Default constructor. Note that the components are not initialized.
    CQuat<T>();

    // Initialize with a multiple of the unity quaternion.
    // Also allows to use 0 as null quaternion.
    CQuat<T>(T s);

    CQuat<T>(T t, T x, T y, T z);
    CQuat<T>(T t, const CVec3<T> &v);

    CQuat<T>(const CQuat<T> &q);// Copy constructor.
    CQuat<T> &operator=(const CQuat<T> &q);// Assign operator.
    CVec3<T> GetVec() const;// Return the components x,y,z as vector.

    void SetVec(const CVec3<T> &v);// Set the x,y,z components and let t unchanged.

    CQuat<T> operator+() const;
    CQuat<T> operator-() const;    // Negation.
    CQuat<T> &operator+=(const CQuat<T> &q);
    CQuat<T> &operator-=(const CQuat<T> &q);
    CQuat<T> &operator*=(const CQuat<T> &q);   // Quaternion multiplication.
	CQuat<T> &operator*=(T s);
	CQuat<T> &operator/=(T s);
    CQuat<T> GetAbs() const;// If t < 0, negate.

    // Negate x,y,z. 
    // Geometrically this means to reverse the rotation
    // but keep the scaling.
    // Mathematically, this is the star operator.
    // See also @ident{CQuat::GetRevMat}.
    CQuat<T> GetRev() const;

    // Get the inverse quaternion.
    CQuat<T> GetInv() const;

    // The sum of the square of each component.
    // This is the scaling factor.
    T GetSqLen() const;

    // The root of the sum of the square of each component.
    // This is the square root of the scaling factor.
    T GetLen() const;

    // The half angle of the rotation.
    // See also @ident{CQuat::GetRotAngle}.
    T GetQuatAngle() const;

    
    T GetRotAngle() const;// The angle of the rotation.
    CQuat<T> GetNormalized() const;// Normalize to length one.
    CQuat<T> GetNormalizedTo(T Len) const;// Normalize to length

    // Square root of the quaternion.
    // Has same axis, half angle, and root of length.
    CQuat<T> GetSqRoot() const;

    // Power of a quaternion, where the result is normalized to one.
    // Has same axis, the angle multiplied by the exponent,
    // and the length always one.
    // If *this is zero, the result is 1.
    // This function is faster than the full CQuat::PowerTo
    CQuat<T> NormalizedPowerTo(T Exponent) const;

    // Power of a quaternion.
    // Has same axis, the angle multiplied by the exponent,
    // and the length raised to the power of the exponent.
    // This function is slower than CQuat::NormalizedPowerTo.
    CQuat<T> PowerTo(T Exponent) const;

    
    bool IsNull() const;// Are all components null?
    bool IsFinite() const;// Are all components finite?
    CMat3x3<T> GetMat() const;// Builds the corresponding scaled rotation matrix.

    // Builds the reverse of the corresponding scaled rotation matrix.
    // See also CQuat::GetRev.
    CMat3x3<T> GetRevMat() const;

    // Builds the inverse of the corresponding scaled rotation matrix.
    // See also CQuat::GetInv.
    CMat3x3<T> GetInvMat() const;

    // Return rotation around Axis by angle Angle radians.
    static CQuat<T> ByRot(
                    const CVec3<T> &Axis, T Angle);

    // Return scaled rotation around Axis 
    // by angle Angle radians and scaled by SqLen.
    static CQuat<T> ByScaledRot(
                    const CVec3<T> &Axis, T Angle, T Scale);

    // Returns the shortest scaled rotation mapping 
    // From into To.
    // Also see CQuat::ByPathAxis 
    // and CQuat::ByPathAlso.
    static CQuat<T> ByPathShortest(
                    const CVec3<T> &From, 
                    const CVec3<T> &To);

    // Returns the rotation around Axis
    // mapping the plane spanned by Axis and From 
    // into the plane spanned by Axis and To.
    // The resulting quaternion has no scale, so it has always length one.
    static CQuat<T> ByPathAxis(
                    const CVec3<T> &Axis,
                    const CVec3<T> &AlsoFrom, 
                    const CVec3<T> &AlsoTo);

    // Returns the scaled rotation mapping From into To
    // but also mapping the plane spanned by From and AlsoFrom 
    // into the plane spanned by To and AlsoTo.
    // Roughly speaking, it mappes From into To,
    // but also tries to map AlsoFrom into AlsoTo
    // as far as possible.
    static CQuat<T> ByPathAlso(const CVec3<T> &From, 
                    const CVec3<T> &To,
                    const CVec3<T> &AlsoFrom, 
                    const CVec3<T> &AlsoTo);

    // Some kind of inverse of CQuat::GetMat.
    // For a quaternion q with must have t not near 0 
    // is CQuat<T>::ByBestOfMat(q.GetMat()) = q.GetAbs().
    // Returns a rotation-invariant result for matrices 
    // which are no scaled rotation.
    // It uses the formulars 
    // SqRoot(0.5 * (m.GetTrace() - s), v = - 0.5 * m.GetDual())
    // Warning! Degenerates for 180 degree rotations (hence t -> 0).
    static CQuat<T> ByBestOfMat(const CMat3x3<T> &m);

    // Some kind of inverse of @ident{CQuat::GetMat}.
    // For a quaternion q 
    // is CQuat<T>::ByRotMat(q.GetMat()) = q.GetAbs().
    // This function has no degeneration point for scaledrotation matrices.
    // Warning! Generates crazy, non-rotation-invariant results 
    // for non-scaledrotation matrices. 
    static CQuat<T> ByRotMat(const CMat3x3<T> &m);

    // Returns the total quaternion when concatenating 
    // the following rotations:
    // 
    // Pan radians around the x-axis.
    // Tilt radians around the y-axis.
    // Roll radians around the z-axis.
    // This is equivalent to
    // 
    // {
    //      CQuat::ByRotXYZ(Pan, Tilt, Roll)
    //        = CQuat::ByRot(CVec3(1, 0, 0), Pan)
    //        * CQuat::ByRot(CVec3(0, 1, 0), Tilt)
    //        * CQuat::ByRot(CVec3(0, 0, 1), Roll)
    // }
    // Assuming that a vector is rotated by multiplying the rotation matrix
    // from left as in
    // {
    //      v -> CQuat::ByRotXYZ(Pan, Tilt, Roll) * v;
    // }
    // pan, tilt and roll 
    // have the usual geometrical interpretation.
    // The inverse operation is CQuat::GetRotAngleXYZ.
    // If the rotations around the axis should be performed 
    // around the axes in different order, you can still use this function, 
    // but have to permutate the x,y and z component 
    // of the resulting quaternion.
    // For example, to rotate 
    // { By Pan around y-axis (instead of x-axis). }
    // { By Tilt around the x-axis (instead of y-axis). }
    // { By Roll around the z-axis. }
    // calculate CQuat::ByRotXYZ(Pan, Tilt, Roll),
    // and swap the x and y component of the resulting quaternion.
    static CQuat<T> ByRotXYZ(T Pan, T Tilt, T Roll);

    // Calculates three rotation angles pan, tilt roll.
    // This is the inverse to CQuat::ByRotXYZ.
    void GetRotAngleXYZ(T &Pan, T &Tilt, T &Roll);

    CQuat<T> operator+(const CQuat<T> &q) const;
    CQuat<T> operator-(const CQuat<T> &q) const;

    CQuat<T> operator*(T s) const;    // Scalar multiplication.
    CQuat<T> operator/(T s) const;    // Scalar division.
    CQuat<T> operator*(const CQuat<T> &q) const;    // Quaternion multiplication.
	

    // Multiply the vector x,y,z with the matrix from right.
    // Return [q.t, q.v * m]
    CQuat<T> operator*(const CMat3x3<T> &m) const;

    // Scalar multiplication with t, cross product from right with x,y,z.
    // Return [v * q.t, v ^ q.v].
    // Warning! This operator does not rotate and scale the vector 
    // by the scaled rotation representated by the quaternion.
    // To perform this, you must use the matrix 
    // returned by CQuat::GetMat.
    CVec3<T> operator*(const CVec3<T> &v) const;

    bool operator==(const CQuat<T> &q) const;
    bool operator!=(const CQuat<T> &q) const;

    // Defines an order on vectors.
    int CompareComp(const CQuat<T> &q) const;

    // Scalar product: 
    // Sum of the product of each corresponding component.
    // This is a rotation invariant quantity.
    // The scalar product is negative if the quotient of the quaternions
    // is negative.
    T ScalarProd(const CQuat<T> &q) const;

    
    T GetQuatAngleBetween(const CQuat<T> &q) const;// Rotation angle between two quaternions:
    T GetRotAngleBetween(const CQuat<T> &q) const;// Rotation angle between two quaternions:

    // interpolation 
    // Interpolates linear between two quaternions.
    // Returns (*this) + (q - (*this)) * Frac.
    // Linear interpolation means linear in the quaternion 
    // considered as vector.
    // Frac == 0 returns *this,
    // Frac == 1 returns q.
    // Note that the corresponding matrix CQuat::GetMat
    // is interpolated quadratically.
    // If *this is near to q relative to their length,
    // linear interpolation is aproximately equivalent 
    // to the exponential and geometric interpolations 
    // CQuat::InterpStrictExp, 
    // CQuat::InterpShortestExp,
    // to CQuat::InterpStrictGeo, and
    // CQuat::InterpShortestGeo 
    CQuat<T> InterpLinear(const CQuat<T> &q, T Frac) const;

    // interpolation 
    // Interpolate exponential between two quaternions.
    // Exponential interpolation means angle-linear interpolation
    // of the rotation, and exponential interpolation of the scale,
    // analogous to exponential interpolation of complex numbers.
    // Frac == 0 returns q1.
    // Frac == 1 returns q2.
    // In contrast to CQuat::InterpShortestExp,
    // interpolation pathes can be rotations up to 360 degrees
    // due to the two-way representation of the same rotation 
    // by two quaternions.
    // In contrast to CQuat::InterpStrictGeo,
    // the scale is interpolated exponential.
    // Both *this and q must not be zero or near zero,
    // since the function degenerates at these points.
    // If *this is near to q relative to their length,
    // the interpolation is aproximately equivalent 
    // to linear interpolation CQuat::InterpLinear.
    CQuat<T> InterpStrictExp(const CQuat<T> &q, T Frac) const;

    // interpolation
    // Interpolate exponential between two scaled rotations.
    // Exponential interpolation means angle-linear interpolation
    // of the rotation, and exponential interpolation of the scale,
    // analogous to exponential interpolation of complex numbers.
    // Frac == 0 returns +/- q1.
    // Frac == 1 returns +/- q2.
    // In contrast to CQuat::InterpStrictExp,
    // this interpolation identifies the two quaternions representing
    // the same scaled rotation, so that the interpolation path
    // always is not more than 180 degree.
    // In contrast to CQuat::InterpShortestExp.
    // the scale is interpolated exponential.
    // Both *this and q must not be zero or near zero,
    // since the function degenerates at these points.
    // If *this is near to q relative to their length,
    // the interpolation is aproximately equivalent 
    // to linear interpolation CQuat::InterpLinear.
    CQuat<T> InterpShortestExp(const CQuat<T> &q, T Frac) const;

    // interpolation 
    // Interpolate geometrical between two quaternions.
    // Geometrical interpolation means angle-linear interpolation
    // of the rotation, and linear interpolation of the scale.
    // Frac == 0} returns q1.
    // Frac == 1} returns q2.
    // In contrast to CQuat::InterpShortestGeo,
    // interpolation pathes can be rotations up to 360 degrees
    // due to the two-way representation of the same rotation 
    // by two quaternions.
    // In contrast to CQuat::InterpStrictExp,
    // the scale is interpolated linear.
    // If *this or q is zero, the angle is kept constant,
    // and only the scale is interpolated.
    // If *this or q are not zero, they should be not near zero
    // to avoid degeneration artifacts.
    // If *this is near to q relative to their length,
    // the interpolation is aproximately equivalent 
    // to linear interpolation CQuat::InterpLinear.
    CQuat<T> InterpStrictGeo(const CQuat<T> &q, T Frac) const;

    // interpolation 
    // Interpolate geometrical between two scaled rotations.
    // Geometrical interpolation means angle-linear interpolation
    // of the rotation, and linear interpolation of the scale.
    // Frac == 0 returns +/- q1.
    // Frac == 1 returns +/- q2.
    // In contrast to CQuat::InterpStrictGeo,
    // this interpolation identifies the two quaternions representing
    // the same scaled rotation, so that the interpolation path
    // always is not more than 180 degree.
    // In contrast to CQuat::InterpShortestExp.
    // the scale is interpolated linear.
    // If *this or q is zero, the angle is kept constant,
    // and only the scale is interpolated.
    // If *this or q are not zero, they should be not near zero
    // to avoid degeneration artifacts.
    // If *this is near to q relative to their length,
    // the interpolation is aproximately equivalent 
    // to linear interpolation CQuat::InterpLinear.
    CQuat<T> InterpShortestGeo(const CQuat<T> &q, T Frac) const;

    // Extract the rotation around the axis Dir. 
    // The rotation axis CQuat::GetVec is projected 
    // onto Dir. It is v.GetProjAlong(Dir) 
    // = CQuat(v.t, v.GetVec().GetProjAlong(Dir)).
    // It is except for rounding errors
    // v.GetProjAlong().GetVec() parallel to Dir.
    // The function does not conserve the norm of the quaternion.
    // See also CQuat::GetProjOrthoLeft
    // and CQuat::GetProjOrthoRight.
    CQuat<T> GetProjAlong(const CVec3<T> &Dir) const;

    // Eliminate rotation around the axis Dir.
    // It is except for rounding errors 
    // v = v.GetProjOrthoLeft() * v.GetProjAlong()
    // and v.GetProjOrthoLeft().GetVec() * Ortho = 0.
    // The function does not conserve the norm of the quaternion.
    // See also CQuat::GetProjAlong
    // and CQuat::GetProjOrthoRight.
    CQuat<T> GetProjOrthoLeft(const CVec3<T> &Dir) const;

    // Eliminate rotation around the axis Dir.
    // It is except for rounding errors 
    // v = v.GetProjAlong() * v.GetProjOrthoRight()
    // and v.GetProjOrthoRight().GetVec() * Ortho = 0.
    // The function does not conserve the norm of the quaternion.
    // See also CQuat::GetProjAlong
    // and CQuat::GetProjOrthoLeft.
    CQuat<T> GetProjOrthoRight(const CVec3<T> &Dir) const;
};


/*
 * CVector Implementation
 */

template<typename T>
inline CVec3<T>::CVec3()
{
}

template<typename T>
inline CVec3<T>::CVec3(T s)
{
    m_x = m_y = m_z = s;
}

template<typename T>
inline CVec3<T>::CVec3(T x, T y, T z)
{
    m_x = x;
    m_y = y;
    m_z = z;
}

template<typename T>
inline CVec3<T>::CVec3(const CVec3<T> &v)
{
    m_x = v.m_x;
    m_y = v.m_y;
    m_z = v.m_z;
}

template<typename T>
inline CVec3<T> &CVec3<T>::operator=(const CVec3<T> &v)
{
    m_x = v.m_x;
    m_y = v.m_y;
    m_z = v.m_z;
    return (*this);
}

template<typename T>
inline CVec3<T> CVec3<T>::operator+() const
{
    return (*this);
}

template<typename T>
inline CVec3<T> CVec3<T>::operator-() const
{
    return CVec3<T>(- m_x, - m_y, - m_z);
}

template<typename T>
inline CVec3<T> &CVec3<T>::operator+=(const CVec3<T> &v)
{
    return (*this) = (*this) + v;
}

template<typename T>
inline CVec3<T> &CVec3<T>::operator-=(const CVec3<T> &v)
{
    return (*this) = (*this) - v;
}

template<typename T>
inline CVec3<T> &CVec3<T>::operator*=(T s)
{
    return (*this) = (*this) * s;
}

template<typename T>
inline CVec3<T> &CVec3<T>::operator/=(T s)
{
    return (*this) = (*this) / s;
}

template<typename T>
inline T CVec3<T>::GetSqLen() const
{
    return m_x * m_x + m_y * m_y + m_z * m_z;
}

template<typename T>
T CVec3<T>::GetLen() const
{
    return Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}

template<typename T>
CVec3<T> CVec3<T>::GetNormalized() const
{
    T f = 1 / Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
    return CVec3<T>(f * m_x, f * m_y, f * m_z);
}

template<typename T>
CVec3<T> CVec3<T>::GetNormalizedTo(T Len) const
{
    T f = Len / Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
    return CVec3<T>(f * m_x, f * m_y, f * m_z);
}

template<typename T>
inline bool CVec3<T>::IsNull() const
{
    return m_x == 0 && m_y == 0 && m_z == 0;
}

template<typename T>
bool CVec3<T>::IsFinite() const
{
    return IsFinite(m_x) && ll3d::IsFinite(m_y) && ll3d::IsFinite(m_z);
}

template<typename T>
CVec3<T> CVec3<T>::InterpLinear(const CVec3<T> &v, T Frac) const
{
    return (*this) + (v - (*this)) * Frac;
}

template<typename T>
CVec3<T> CVec3<T>::GetProjAlong(const CVec3<T> &Dir) const
{
    const CVec3<T> &Vec = (*this);
    return Dir * ((Vec * Dir) / Dir.GetSqLen());
}

template<typename T>
CVec3<T> CVec3<T>::GetProjOrtho(const CVec3<T> &Dir) const
{
    const CVec3<T> &Vec = (*this);
    return Vec - Dir * ((Vec * Dir) / Dir.GetSqLen());
}

template<typename T>
CVec3<T> CVec3<T>::GetConstrAlong(const CVec3<T> &Dir) const
{
    const CVec3<T> &Vec = (*this);
    return Dir * (Vec.GetSqLen() / (Vec * Dir));
}

template<typename T>
CVec3<T> CVec3<T>::GetConstrOrtho(const CVec3<T> &Dir) const
{
    const CVec3<T> &Vec = (*this);
    T Frac = Vec.GetSqLen() / (Dir ^ Vec).GetSqLen();
    return Vec * (Frac * Dir.GetSqLen()) - Dir * (Frac * (Dir * Vec));
}

template<typename T>
CVec3<T> CVec3<T>::GetReziproc() const
{
    T f = 1 / (m_x * m_x + m_y * m_y + m_z * m_z);
    return CVec3<T>(f * m_x, f * m_y, f * m_z);
}

template<typename T>
inline CVec3<T> CVec3<T>::GetAbsComp() const
{
    return CVec3<T>(Abs(m_x), Abs(m_y), Abs(m_z));
}

template<typename T>
CVec3<T> CVec3<T>::GetSignComp() const
{
    return CVec3<T>(Sign(m_x), Sign(m_y), Sign(m_z));
}

template<typename T>
T CVec3<T>::GetMinComp() const
{
    return Min(m_x, m_y, m_z);
}

template<typename T>
T CVec3<T>::GetMaxComp() const
{
    return Max(m_x, m_y, m_z);
}

template<typename T>
T CVec3<T>::GetSumAbsComp() const
{
    return Abs(m_x) + Abs(m_y) + Abs(m_z);
}

template<typename T>
T CVec3<T>::GetMaxAbsComp() const
{
    return Max(Abs(m_x), Abs(m_y), Abs(m_z));
}

template<typename T>
CVec3<T> CVec3<T>::CompMin(const CVec3<T> &v) const
{
    return CVec3<T>(
            Min(m_x, v.m_x), 
            Min(m_y, v.m_y),
            Min(m_z, v.m_z));
}

template<typename T>
CVec3<T> CVec3<T>::CompMax(const CVec3<T> &v) const
{
    return CVec3<T>(
            Max(m_x, v.m_x), 
            Max(m_y, v.m_y),
            Max(m_z, v.m_z));
}

template<typename T>
CVec3<T> CVec3<T>::CompMin(const CVec3<T> &v2,
                                     const CVec3<T> &v3) const
{
    return CVec3<T>(
            Min(m_x, v2.m_x, v3.m_x),
            Min(m_y, v2.m_y, v3.m_y),
            Min(m_z, v2.m_z, v3.m_z));
}

template<typename T>
CVec3<T> CVec3<T>::CompMax(const CVec3<T> &v2,
                                     const CVec3<T> &v3) const
{
    return CVec3<T>(
            Max(m_x, v2.m_x, v3.m_x),
            Max(m_y, v2.m_y, v3.m_y),
            Max(m_z, v2.m_z, v3.m_z));
}

template<typename T>
CVec3<T> &CVec3<T>::SetCompToMin(const CVec3<T> &v)
{
    if(v.m_x < m_x) m_x = v.m_x;
    if(v.m_y < m_y) m_y = v.m_y;
    if(v.m_z < m_z) m_z = v.m_z;
    return (*this);
}

template<typename T>
CVec3<T> &CVec3<T>::SetCompToMax(const CVec3<T> &v)
{
    if(v.m_x > m_x) m_x = v.m_x;
    if(v.m_y > m_y) m_y = v.m_y;
    if(v.m_z > m_z) m_z = v.m_z;
    return (*this);
}

///////////////////////////////////////////////////////////////////////////////

template<typename T>
inline CVec3<T> CVec3<T>::operator+(const CVec3<T> &v) const
{
    return CVec3<T>(m_x + v.m_x, m_y + v.m_y, m_z + v.m_z);
}

template<typename T>
inline CVec3<T> CVec3<T>::operator-(const CVec3<T> &v) const
{
    return CVec3<T>(m_x - v.m_x, m_y - v.m_y, m_z - v.m_z);
}

template<typename T>
inline CVec3<T> CVec3<T>::operator*(T s) const
{
    return CVec3<T>(m_x * s, m_y * s, m_z * s);
}

template<typename T>
inline CVec3<T> CVec3<T>::operator/(T s) const
{
    return CVec3<T>(m_x / s, m_y / s, m_z / s);
}

template<typename T>
inline T CVec3<T>::operator*(const CVec3<T> &v) const
{
    return m_x * v.m_x + m_y * v.m_y + m_z * v.m_z;
}

template<typename T>
inline CVec3<T> CVec3<T>::operator^(const CVec3<T> &v) const
{
    return CVec3<T>(
        m_y * v.m_z - m_z * v.m_y,
        m_z * v.m_x - m_x * v.m_z,
        m_x * v.m_y - m_y * v.m_x);
}

template<typename T>
inline CVec3<T> CVec3<T>::operator*(const CMat3x3<T> &m) const
{
    return CVec3<T>(
        m_x * m.m_xx + m_y * m.m_yx + m_z * m.m_zx,
        m_x * m.m_xy + m_y * m.m_yy + m_z * m.m_zy,
        m_x * m.m_xz + m_y * m.m_yz + m_z * m.m_zz);
}

template<typename T>
inline bool CVec3<T>::operator==(const CVec3<T> &v) const
{
    return m_x == v.m_x && m_y == v.m_y && m_z == v.m_z;
}

template<typename T>
inline bool CVec3<T>::operator!=(const CVec3<T> &v) const
{
    return m_x != v.m_x || m_y != v.m_y || m_z != v.m_z;
}

template<typename T>
int CVec3<T>::CompareComp(const CVec3<T> &v) const
{
    if(m_x < v.m_x)
        return -1;
    else if(m_x > v.m_x)
        return 1;
    else if(m_y < v.m_y)
        return -1;
    else if(m_y > v.m_y)
        return 1;
    else if(m_z < v.m_z)
        return -1;
    else if(m_z > v.m_z)
        return 1;
    else
        return 0;
}

template<typename T>
T CVec3<T>::Area(const CVec3<T> &v) const
{
    T x = m_y * v.m_z - m_z * v.m_y;
    T y = m_z * v.m_x - m_x * v.m_z;
    T z = m_x * v.m_y - m_y * v.m_x;
    return Sqrt(x * x + y * y + z * z);
}

template<typename T>
T CVec3<T>::Volume(const CVec3<T> &v2, 
                              const CVec3<T> &v3) const
{
    // Not using subdeterminant expansion to guarantee 
    // that the result is exact null if two vectors are equal.
    return m_x * v2.m_y * v3.m_z - m_x * v2.m_z * v3.m_y 
         + m_y * v2.m_z * v3.m_x - m_y * v2.m_x * v3.m_z 
         + m_z * v2.m_x * v3.m_y - m_z * v2.m_y * v3.m_x;
}

template<typename T>
inline CVec3<T> CVec3<T>::CompProd(const CVec3<T> &v) const
{
    return CVec3<T>(m_x * v.m_x, m_y * v.m_y, m_z * v.m_z);
}

template<typename T>
inline CVec3<T> CVec3<T>::CompQuot(const CVec3<T> &v) const
{
    return CVec3<T>(m_x / v.m_x, m_y / v.m_y, m_z / v.m_z);
}

template<typename T>
inline CVec3<T> CVec3<T>::CompReziproc() const
{
    return CVec3<T>(1 / m_x, 1 / m_y, 1 / m_z);
}

template<typename T>
bool CVec3<T>::IsCosAngleAbove(const CVec3<T> &v, T c) const
{
    const CVec3<T> &This = (*this);
    T Scal = This * v;
    T Sq1 = GetSqLen();
    T Sq2 = v.GetSqLen();
    return Scal * Abs(Scal) > c * Sq1 * Sq2;
}

template<typename T>
bool CVec3<T>::IsEachCompNe(const CVec3<T> &v) const
{
    return m_x != v.m_x && m_y != v.m_y && m_z != v.m_z;
}

template<typename T>
bool CVec3<T>::IsEachCompLt(const CVec3<T> &v) const
{
    return m_x < v.m_x && m_y < v.m_y && m_z < v.m_z;
}

template<typename T>
bool CVec3<T>::IsEachCompLe(const CVec3<T> &v) const
{
    return m_x <= v.m_x && m_y <= v.m_y && m_z <= v.m_z;
}

template<typename T>
bool CVec3<T>::IsEachCompGt(const CVec3<T> &v) const
{
    return m_x > v.m_x && m_y > v.m_y && m_z > v.m_z;
}

template<typename T>
bool CVec3<T>::IsEachCompGe(const CVec3<T> &v) const
{
    return m_x >= v.m_x && m_y >= v.m_y && m_z >= v.m_z;
}

template<typename T>
bool CVec3<T>::IsAnyCompEq(const CVec3<T> &v) const
{
    return m_x == v.m_x || m_y == v.m_y || m_z == v.m_z;
}

template<typename T>
bool CVec3<T>::IsAnyCompLt(const CVec3<T> &v) const
{
    return m_x < v.m_x || m_y < v.m_y || m_z < v.m_z;
}

template<typename T>
bool CVec3<T>::IsAnyCompLe(const CVec3<T> &v) const
{
    return m_x <= v.m_x || m_y <= v.m_y || m_z <= v.m_z;
}

template<typename T>
bool CVec3<T>::IsAnyCompGt(const CVec3<T> &v) const
{
    return m_x > v.m_x || m_y > v.m_y || m_z > v.m_z;
}

template<typename T>
bool CVec3<T>::IsAnyCompGe(const CVec3<T> &v) const
{
    return m_x >= v.m_x || m_y >= v.m_y || m_z >= v.m_z;
}


/*
 * CMat3x3 Implementation
 */

template<typename T>
inline CMat3x3<T>::CMat3x3()
{
}

template<typename T>
inline CMat3x3<T>::CMat3x3(T s)
{
    m_xx = s;  m_xy = 0;  m_xz = 0;
    m_yx = 0;  m_yy = s;  m_yz = 0;
    m_zx = 0,  m_zy = 0;  m_zz = s;
}

template<typename T>
inline CMat3x3<T>::CMat3x3(T xx, T xy, T xz, 
                                     T yx, T yy, T yz, 
                                     T zx, T zy, T zz)
{
    m_xx = xx;  m_xy = xy;  m_xz = xz;
    m_yx = yx;  m_yy = yy;  m_yz = yz;
    m_zx = zx;  m_zy = zy;  m_zz = zz;
}

template<typename T>
inline CMat3x3<T>::CMat3x3(const CMat3x3<T> &m)
{
    m_xx = m.m_xx;  m_xy = m.m_xy;  m_xz = m.m_xz;
    m_yx = m.m_yx;  m_yy = m.m_yy;  m_yz = m.m_yz;
    m_zx = m.m_zx;  m_zy = m.m_zy;  m_zz = m.m_zz;
}

template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator=(const CMat3x3<T> &m)
{
    m_xx = m.m_xx;  m_xy = m.m_xy;  m_xz = m.m_xz;
    m_yx = m.m_yx;  m_yy = m.m_yy;  m_yz = m.m_yz;
    m_zx = m.m_zx;  m_zy = m.m_zy;  m_zz = m.m_zz;
    return (*this);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator+() const
{
    return (*this);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator-() const
{
    return CMat3x3<T>(
        - m_xx, - m_xy, - m_xz, 
        - m_yx, - m_yy, - m_yz, 
        - m_zx, - m_zy, - m_zz);
}

template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator+=(const CMat3x3<T> &m)
{
    return (*this) = (*this) + m;
}

template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator-=(const CMat3x3<T> &m)
{
    return (*this) = (*this) - m;
}

template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator*=(const CMat3x3<T> &m)
{
    return (*this) = (*this) * m;
}

template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator*=(T s)
{
    return (*this) = (*this) * s;
}

template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator/=(T s)
{
    return (*this) = (*this) / s;
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::GetTransposed() const
{
    return CMat3x3<T>(
        m_xx, m_yx, m_zx, 
        m_xy, m_yy, m_zy, 
        m_xz, m_yz, m_zz);
}

template<typename T>
CMat3x3<T> CMat3x3<T>::GetSubDet() const
{
    return CMat3x3<T>(
        m_yy * m_zz - m_yz * m_zy,
        m_yz * m_zx - m_yx * m_zz,
        m_yx * m_zy - m_yy * m_zx,
        m_zy * m_xz - m_zz * m_xy,
        m_zz * m_xx - m_zx * m_xz,
        m_zx * m_xy - m_zy * m_xx,
        m_xy * m_yz - m_xz * m_yy,
        m_xz * m_yx - m_xx * m_yz,
        m_xx * m_yy - m_xy * m_yx);
}

template<typename T>
CMat3x3<T> CMat3x3<T>::GetInv() const
{
    T sxx = m_yy * m_zz - m_yz * m_zy;
    T sxy = m_yz * m_zx - m_yx * m_zz;
    T sxz = m_yx * m_zy - m_yy * m_zx;
    T syx = m_zy * m_xz - m_zz * m_xy;
    T syy = m_zz * m_xx - m_zx * m_xz;
    T syz = m_zx * m_xy - m_zy * m_xx;
    T szx = m_xy * m_yz - m_xz * m_yy;
    T szy = m_xz * m_yx - m_xx * m_yz;
    T szz = m_xx * m_yy - m_xy * m_yx;
    T d = m_xx * sxx + m_xy * sxy + m_xz * sxz;
    T f = 1 / d;
    return CMat3x3<T>(
        f * sxx, f * syx, f * szx, 
        f * sxy, f * syy, f * szy, 
        f * sxz, f * syz, f * szz);
}

template<typename T>
T CMat3x3<T>::GetSqLen() const
{
    return (m_xx * m_xx + m_xy * m_xy + m_xz * m_xz)
         + (m_yx * m_yx + m_yy * m_yy + m_yz * m_yz)
         + (m_zx * m_zx + m_zy * m_zy + m_zz * m_zz);
}

template<typename T>
T CMat3x3<T>::GetLen() const
{
    return Sqrt(GetSqLen());
}

template<typename T>
T CMat3x3<T>::GetDet() const
{
    // Not using subdeterminant expansion to guarantee 
    // that the result is exact null if two vectors are equal.
    return m_xx * m_yy * m_zz - m_xx * m_yz * m_zy 
         + m_xy * m_yz * m_zx - m_xy * m_yx * m_zz 
         + m_xz * m_yx * m_zy - m_xz * m_yy * m_zx;
}

template<typename T>
inline T CMat3x3<T>::GetTrace() const
{
    return m_xx + m_yy + m_zz;
}

template<typename T>
inline CVec3<T> CMat3x3<T>::GetDual() const
{
    return CVec3<T>(m_yz - m_zy, m_zx - m_xz, m_xy - m_yx);
}

template<typename T>
bool CMat3x3<T>::IsNull() const
{
    return m_xx == 0 && m_xy == 0 && m_xz == 0
        && m_yx == 0 && m_yy == 0 && m_yz == 0
        && m_zx == 0 && m_zy == 0 && m_zz == 0;
};

template<typename T>
bool CMat3x3<T>::IsFinite() const
{
    return IsFinite(m_xx) && IsFinite(m_xy) && IsFinite(m_xz)
        && IsFinite(m_yx) && IsFinite(m_yy) && IsFinite(m_yz)
        && IsFinite(m_zx) && IsFinite(m_zy) && IsFinite(m_zz);
};

template<typename T>
CVec3<T> CMat3x3<T>::GetRowX() const 
{ 
    return CVec3<T>(m_xx, m_xy, m_xz); 
}

template<typename T>
CVec3<T> CMat3x3<T>::GetRowY() const 
{ 
    return CVec3<T>(m_yx, m_yy, m_yz);
}

template<typename T>
CVec3<T> CMat3x3<T>::GetRowZ() const 
{ 
    return CVec3<T>(m_zx, m_zy, m_zz); 
}

template<typename T>
CVec3<T> CMat3x3<T>::GetColX() const 
{ 
    return CVec3<T>(m_xx, m_yx, m_zx);
}

template<typename T>
CVec3<T> CMat3x3<T>::GetColY() const 
{ 
    return CVec3<T>(m_xy, m_yy, m_zy); 
}

template<typename T>
CVec3<T> CMat3x3<T>::GetColZ() const 
{ 
    return CVec3<T>(m_xz, m_yz, m_zz); 
}

template<typename T>
void CMat3x3<T>::SetRowX(const CVec3<T> &v)
{
    m_xx = v.m_x;
    m_xy = v.m_y;
    m_xz = v.m_z;
}

template<typename T>
void CMat3x3<T>::SetRowY(const CVec3<T> &v)
{
    m_yx = v.m_x;
    m_yy = v.m_y;
    m_yz = v.m_z;
}

template<typename T>
void CMat3x3<T>::SetRowZ(const CVec3<T> &v)
{
    m_zx = v.m_x;
    m_zy = v.m_y;
    m_zz = v.m_z;
}

template<typename T>
void CMat3x3<T>::SetColX(const CVec3<T> &v)
{
    m_xx = v.m_x;
    m_yx = v.m_y;
    m_zx = v.m_z;
}

template<typename T>
void CMat3x3<T>::SetColY(const CVec3<T> &v)
{
    m_xy = v.m_x;
    m_yy = v.m_y;
    m_zy = v.m_z;
}

template<typename T>
void CMat3x3<T>::SetColZ(const CVec3<T> &v)
{
    m_xz = v.m_x;
    m_yz = v.m_y;
    m_zz = v.m_z;
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::GetAbsComp() const
{
    return CMat3x3<T>(
        Abs(m_xx), 
        Abs(m_xy), 
        Abs(m_xz),
        Abs(m_yx), 
        Abs(m_yy), 
        Abs(m_yz),
        Abs(m_zx), 
        Abs(m_zy), 
        Abs(m_zz));
}

template<typename T>
inline T CMat3x3<T>::GetSumAbsComp() const
{
    return Abs(m_xx) + Abs(m_xy) + Abs(m_xz)
         + Abs(m_yx) + Abs(m_yy) + Abs(m_yz)
         + Abs(m_zx) + Abs(m_zy) + Abs(m_zz);
}

template<typename T>
inline T CMat3x3<T>::GetMaxAbsComp() const
{
    return Max(
                Max(Abs(m_xx), Abs(m_xy), Abs(m_xz)),
                Max(Abs(m_yx), Abs(m_yy), Abs(m_yz)),
                Max(Abs(m_zx), Abs(m_zy), Abs(m_zz)));
}

template<typename T>
CVec3<T> CMat3x3<T>::GetSumAbsRows() const
{
    return CVec3<T>(
        Abs(m_xx) + Abs(m_xy) + Abs(m_xz),
        Abs(m_yx) + Abs(m_yy) + Abs(m_yz),
        Abs(m_zx) + Abs(m_zy) + Abs(m_zz));
}

template<typename T>
CVec3<T> CMat3x3<T>::GetSumAbsCols() const
{
    return CVec3<T>(
        Abs(m_xx) + Abs(m_yx) + Abs(m_zx),
        Abs(m_xy) + Abs(m_yy) + Abs(m_zy),
        Abs(m_xz) + Abs(m_yz) + Abs(m_zz));
}

template<typename T>
CVec3<T> CMat3x3<T>::GetMaxAbsRows() const
{
    return CVec3<T>(
        Max(Abs(m_xx), Abs(m_xy), Abs(m_xz)),
        Max(Abs(m_yx), Abs(m_yy), Abs(m_yz)),
        Max(Abs(m_zx), Abs(m_zy), Abs(m_zz)));
}

template<typename T>
CVec3<T> CMat3x3<T>::GetMaxAbsCols() const
{
    return CVec3<T>(
        Max(Abs(m_xx), Abs(m_yx), Abs(m_zx)),
        Max(Abs(m_xy), Abs(m_yy), Abs(m_zy)),
        Max(Abs(m_xz), Abs(m_yz), Abs(m_zz)));
}

template<typename T>
CVec3<T> CMat3x3<T>::GetLenRows() const
{
    return CVec3<T>(
        Sqrt(m_xx * m_xx + m_xy * m_xy + m_xz * m_xz),
        Sqrt(m_yx * m_yx + m_yy * m_yy + m_yz * m_yz),
        Sqrt(m_zx * m_zx + m_zy * m_zy + m_zz * m_zz));
}

template<typename T>
CVec3<T> CMat3x3<T>::GetLenCols() const
{
    return CVec3<T>(
        Sqrt(m_xx * m_xx + m_yx * m_yx + m_zx * m_zx),
        Sqrt(m_xy * m_xy + m_yy * m_yy + m_zy * m_zy),
        Sqrt(m_xz * m_xz + m_yz * m_yz + m_zz * m_zz));
}

template<typename T>
CMat3x3<T> CMat3x3<T>::GetScaledRows(
                                            const CVec3<T> &v) const
{
    return CMat3x3<T>(
        v.m_x * m_xx, v.m_x * m_xy, v.m_x * m_xz,
        v.m_y * m_yx, v.m_y * m_yy, v.m_y * m_yz,
        v.m_z * m_zx, v.m_z * m_zy, v.m_z * m_zz);
}

template<typename T>
CMat3x3<T> CMat3x3<T>::GetScaledCols(
                                            const CVec3<T> &v) const
{
    return CMat3x3<T>(
        v.m_x * m_xx, v.m_y * m_xy, v.m_z * m_xz,
        v.m_x * m_yx, v.m_y * m_yy, v.m_z * m_yz,
        v.m_x * m_zx, v.m_y * m_zy, v.m_z * m_zz);
}

///////////////////////////////////////////////////////////////////////////////

template<typename T>
CMat3x3<T> CMat3x3<T>::ByTensorProd(
                    const CVec3<T> &v1, const CVec3<T> &v2)
{
    return CMat3x3<T>(
        v1.m_x * v2.m_x, v1.m_x * v2.m_y, v1.m_x * v2.m_z,
        v1.m_y * v2.m_x, v1.m_y * v2.m_y, v1.m_y * v2.m_z,
        v1.m_z * v2.m_x, v1.m_z * v2.m_y, v1.m_z * v2.m_z);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByDual(const CVec3<T> &v)
{
    return CMat3x3<T>(
        0, v.m_z, - v.m_y, - v.m_z, 0,  v.m_x, v.m_y, - v.m_x, 0);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByDiag(const CVec3<T> &v)
{
    return CMat3x3<T>(v.m_x, 0, 0, 0, v.m_y, 0, 0, 0, v.m_z);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByDiag(T x, T y, T z)
{
    return CMat3x3<T>(x, 0, 0, 0, y, 0, 0, 0, z);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByRows(
                        const CVec3<T> &vx, 
                        const CVec3<T> &vy, 
                        const CVec3<T> &vz)
{
    return CMat3x3<T>(
        vx.m_x, vx.m_y, vx.m_z,
        vy.m_x, vy.m_y, vy.m_z,
        vz.m_x, vz.m_y, vz.m_z);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByCols(
                        const CVec3<T> &vx, 
                        const CVec3<T> &vy, 
                        const CVec3<T> &vz)
{
    return CMat3x3<T>(
        vx.m_x, vy.m_x, vz.m_x,
        vx.m_y, vy.m_y, vz.m_y,
        vx.m_z, vy.m_z, vz.m_z);
}

///////////////////////////////////////////////////////////////////////////////

template<typename T>
inline bool CMat3x3<T>::operator==(const CMat3x3<T> &m) const
{
    return m_xx == m.m_xx && m_xy == m.m_xy && m_xz == m.m_xz
        && m_yx == m.m_yx && m_yy == m.m_yy && m_yz == m.m_yz
        && m_zx == m.m_zx && m_zy == m.m_zy && m_zz == m.m_zz;
}

template<typename T>
inline bool CMat3x3<T>::operator!=(const CMat3x3<T> &m) const
{
    return m_xx != m.m_xx || m_xy != m.m_xy || m_xz != m.m_xz
        || m_yx != m.m_yx || m_yy != m.m_yy || m_yz != m.m_yz
        || m_zx != m.m_zx || m_zy != m.m_zy || m_zz != m.m_zz;
}

template<typename T>
int CMat3x3<T>::CompareComp(const CMat3x3<T> &m) const
{
    if(m_xx < m.m_xx)
        return -1;
    else if(m_xx > m.m_xx)
        return 1;
    else if(m_xy < m.m_xy)
        return -1;
    else if(m_xy > m.m_xy)
        return 1;
    else if(m_xz < m.m_xz)
        return -1;
    else if(m_xz > m.m_xz)
        return 1;
    else if(m_yx < m.m_yx)
        return -1;
    else if(m_yx > m.m_yx)
        return 1;
    else if(m_yy < m.m_yy)
        return -1;
    else if(m_yy > m.m_yy)
        return 1;
    else if(m_yz < m.m_yy)
        return -1;
    else if(m_yz > m.m_yz)
        return 1;
    else if(m_zx < m.m_zx)
        return -1;
    else if(m_zx > m.m_zx)
        return 1;
    else if(m_zy < m.m_zy)
        return -1;
    else if(m_zy > m.m_zy)
        return 1;
    else if(m_zz < m.m_zy)
        return -1;
    else if(m_zz > m.m_zz)
        return 1;
    else
        return 0;
}

///////////////////////////////////////////////////////////////////////////////

template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator+(const CMat3x3<T> &m) const
{
    return CMat3x3<T>(
        m_xx + m.m_xx, m_xy + m.m_xy, m_xz + m.m_xz,
        m_yx + m.m_yx, m_yy + m.m_yy, m_yz + m.m_yz,
        m_zx + m.m_zx, m_zy + m.m_zy, m_zz + m.m_zz);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator-(const CMat3x3<T> &m) const
{
    return CMat3x3<T>(
        m_xx - m.m_xx, m_xy - m.m_xy, m_xz - m.m_xz,
        m_yx - m.m_yx, m_yy - m.m_yy, m_yz - m.m_yz,
        m_zx - m.m_zx, m_zy - m.m_zy, m_zz - m.m_zz);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator*(T s) const
{
    return CMat3x3<T>(
        m_xx * s, m_xy * s, m_xz * s,
        m_yx * s, m_yy * s, m_yz * s,
        m_zx * s, m_zy * s, m_zz * s);
}

template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator/(T s) const
{
    return CMat3x3<T>(
        m_xx / s, m_xy / s, m_xz / s,
        m_yx / s, m_yy / s, m_yz / s,
        m_zx / s, m_zy / s, m_zz / s);
}

template<typename T>
CMat3x3<T> CMat3x3<T>::operator*(const CMat3x3<T> &m) const
{
    return CMat3x3<T>(
        m_xx * m.m_xx + m_xy * m.m_yx + m_xz * m.m_zx,
        m_xx * m.m_xy + m_xy * m.m_yy + m_xz * m.m_zy,
        m_xx * m.m_xz + m_xy * m.m_yz + m_xz * m.m_zz,
        m_yx * m.m_xx + m_yy * m.m_yx + m_yz * m.m_zx,
        m_yx * m.m_xy + m_yy * m.m_yy + m_yz * m.m_zy,
        m_yx * m.m_xz + m_yy * m.m_yz + m_yz * m.m_zz,
        m_zx * m.m_xx + m_zy * m.m_yx + m_zz * m.m_zx,
        m_zx * m.m_xy + m_zy * m.m_yy + m_zz * m.m_zy,
        m_zx * m.m_xz + m_zy * m.m_yz + m_zz * m.m_zz);
}

template<typename T>
CVec3<T> CMat3x3<T>::operator*(const CVec3<T> &v) const
{
    return CVec3<T>(
        m_xx * v.m_x + m_xy * v.m_y + m_xz * v.m_z,
        m_yx * v.m_x + m_yy * v.m_y + m_yz * v.m_z,
        m_zx * v.m_x + m_zy * v.m_y + m_zz * v.m_z);
}

/*
 * CQuat Implementation
 */

template<typename T>
inline CQuat<T>::CQuat()
{
}

template<typename T>
inline CQuat<T>::CQuat(T s)
{
    m_t = s;
    m_x = m_y = m_z = 0;
}

template<typename T>
inline CQuat<T>::CQuat(T t, T x, T y, T z)
{
    m_t = t;
    m_x = x;
    m_y = y;
    m_z = z;
}

template<typename T>
inline CQuat<T>::CQuat(T t, const CVec3<T> &v)
{
    m_t = t;
    m_x = v.m_x;
    m_y = v.m_y;
    m_z = v.m_z;
}

template<typename T>
inline CQuat<T>::CQuat(const CQuat<T> &q)
{
    m_t = q.m_t;
    m_x = q.m_x;
    m_y = q.m_y;
    m_z = q.m_z;
}

template<typename T>
inline CVec3<T> CQuat<T>::GetVec() const
{
    return CVec3<T>(m_x, m_y, m_z);
}

template<typename T>
inline void CQuat<T>::SetVec(const CVec3<T> &v)
{
    m_x = v.m_x;
    m_y = v.m_y;
    m_z = v.m_z;
}

template<typename T>
inline CQuat<T> &CQuat<T>::operator=(const CQuat<T> &q)
{
    m_t = q.m_t;
    m_x = q.m_x;
    m_y = q.m_y;
    m_z = q.m_z;
    return (*this);
}

template<typename T>
inline CQuat<T> CQuat<T>::operator+() const
{
    return (*this);
}

template<typename T>
inline CQuat<T> CQuat<T>::operator-() const
{
    return CQuat<T>(- m_t, - m_x, - m_y, - m_z);
}

template<typename T>
inline CQuat<T> &CQuat<T>::operator+=(const CQuat<T> &q)
{
    return (*this) = (*this) + q;
}

template<typename T>
inline CQuat<T> &CQuat<T>::operator-=(const CQuat<T> &q)
{
    return (*this) = (*this) - q;
}

template<typename T>
inline CQuat<T> &CQuat<T>::operator*=(const CQuat<T> &q)
{
    return (*this) = (*this) * q;
}

template<typename T>
inline CQuat<T> &CQuat<T>::operator*=(T s)
{
    return (*this) = (*this) * s;
}

template<typename T>
inline CQuat<T> &CQuat<T>::operator/=(T s)
{
    return (*this) = (*this) / s;
}

template<typename T>
CQuat<T> operator*(const CMat3x3<T> &m, const CQuat<T> &q)
{
    // = [q.t, m * q.v]
    return CQuat<T>(
        q.m_t, 
        m.m_xx * q.m_x + m.m_xy * q.m_y + m.m_xz * q.m_z,
        m.m_yx * q.m_x + m.m_yy * q.m_y + m.m_yz * q.m_z,
        m.m_zx * q.m_x + m.m_zy * q.m_y + m.m_zz * q.m_z);
}

template<typename T>
CQuat<T> operator*(const CQuat<T> &q, const CMat3x3<T> &m)
{
    // = [q.t, q.v * m]
    return CQuat<T>(
        q.m_t,
        q.m_x * m.m_xx + q.m_y * m.m_yx + q.m_z * m.m_zx,
        q.m_x * m.m_xy + q.m_y * m.m_yy + q.m_z * m.m_zy,
        q.m_x * m.m_xz + q.m_y * m.m_yz + q.m_z * m.m_zz);
}

template<typename T>
CVec3<T> operator*(const CVec3<T> &v, const CQuat<T> &q)
{
    // = [v * q.t, v ^ q.v]
    return CVec3<T>(
        v.m_x * q.m_t + v.m_y * q.m_z - v.m_z * q.m_y,
        v.m_y * q.m_t + v.m_z * q.m_x - v.m_x * q.m_z,
        v.m_z * q.m_t + v.m_x * q.m_y - v.m_y * q.m_x);
}

template<typename T>
CVec3<T> operator*(const CQuat<T> &q, const CVec3<T> &v)
{
    // = [q.t * v, q.v ^ v]
    return CVec3<T>(
        q.m_t * v.m_x + q.m_y * v.m_z - q.m_z * v.m_y,
        q.m_t * v.m_y + q.m_z * v.m_x - q.m_x * v.m_z,
        q.m_t * v.m_z + q.m_x * v.m_y - q.m_y * v.m_x);
}

template<typename T>
CQuat<T> CQuat<T>::GetAbs() const
{
    if(m_t >= 0)
        return CQuat<T>(m_t, m_x, m_y, m_z);
    else
        return CQuat<T>(- m_t, - m_x, - m_y, - m_z);
}

template<typename T>
inline CQuat<T> CQuat<T>::GetRev() const
{
    return CQuat<T>(m_t, - m_x, - m_y, - m_z);
}

template<typename T>
CQuat<T> CQuat<T>::GetInv() const
{
    T f = 1 / (m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
    return CQuat<T>(f * m_t, - f * m_x, - f * m_y, - f * m_z);
}

template<typename T>
T CQuat<T>::GetSqLen() const
{
    return m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z;
}

template<typename T>
T CQuat<T>::GetLen() const
{
    return Sqrt(GetSqLen());
}

template<typename T>
inline T CQuat<T>::GetQuatAngle() const
{
    return 0.5 * GetRotAngle();
}

template<typename T>
T CQuat<T>::GetRotAngle() const
{
    T cc = m_t * m_t;
    T ss = m_x * m_x + m_y * m_y + m_z * m_z;
    return Acos((cc - ss) / (cc + ss));
}

template<typename T>
CQuat<T> CQuat<T>::GetNormalized() const
{
    T f = 1 / Sqrt(m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
    return CQuat<T>(f * m_t, f * m_x, f * m_y, f * m_z);
}

template<typename T>
CQuat<T> CQuat<T>::GetNormalizedTo(T Len) const
{
    T f = Len / Sqrt(m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
    return CQuat<T>(f * m_t, f * m_x, f * m_y, f * m_z);
}

template<typename T>
CQuat<T> CQuat<T>::GetSqRoot() const
{
    T Len = Sqrt(m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
    T RootArg = 2 * (Len + m_t);
    if(RootArg < 0) // Maybe caused by precision errors.
        RootArg = 0;
    T Root = Sqrt(RootArg);
    T f = 1 / Root;
    if(!IsFinite(f))
        f = 0;
    return CQuat<T>(0.5 * Root, f * m_x, f * m_y, f * m_z);
}

template<typename T>
CQuat<T> CQuat<T>::NormalizedPowerTo(T Exponent) const
{
    T cc = m_t * m_t;
    T ss = m_x * m_x + m_y * m_y + m_z * m_z;
    T dc = (cc - ss) / (cc + ss);
    if(!IsFinite(dc))
    {
        // *this is near zero.
        return CQuat<T>(1);    
    }
    else
    {
        T Angle = 0.5 * Acos(dc);
        T NewAngle = Exponent * Angle;
        T hs = Sin(NewAngle);
        T hc = Cos(NewAngle);
        T f = hs / Sqrt(ss);
        if(!IsFinite(f))
        {
            // *this is near to 1.
            return CQuat<T>(1);    
        }
        else
            return CQuat<T>(hc, f * m_x, f * m_y, f * m_z);
    }
}

template<typename T>
CQuat<T> CQuat<T>::PowerTo(T Exponent) const
{
    T SqLen = m_t * m_t + + m_x * m_x + m_y * m_y + m_z * m_z;
    T LogSqLen = Log(SqLen);
    if(!IsFinite(LogSqLen))
        return CQuat<T>(0);    // *this was 0.
    else
    {
        T NewLen = Exp(Exponent * 0.5f * LogSqLen);
        return NormalizedPowerTo(Exponent) * NewLen;
    }
}

template<typename T>
inline bool CQuat<T>::IsNull() const
{
    return m_t == 0 
        && m_x == 0 
        && m_y == 0 
        && m_z == 0;
}

template<typename T>
bool CQuat<T>::IsFinite() const
{
    return IsFinite(m_t) 
        && IsFinite(m_x) 
        && IsFinite(m_y) 
        && IsFinite(m_z);
}

template<typename T>
CMat3x3<T> CQuat<T>::GetMat() const
{
    // M w = (t^2 - v^2) w + 2 v (v w) + 2 t (v ^ w)

    T tt = m_t * m_t;
    T xx = m_x * m_x;
    T yy = m_y * m_y;
    T zz = m_z * m_z;

    T tx2 = 2 * m_t * m_x;
    T ty2 = 2 * m_t * m_y;
    T tz2 = 2 * m_t * m_z;
    T xy2 = 2 * m_x * m_y;
    T yz2 = 2 * m_y * m_z;
    T zx2 = 2 * m_z * m_x;
    
    return CMat3x3<T>(
        tt + xx - yy - zz, xy2 - tz2, zx2 + ty2,
        xy2 + tz2, tt + yy - zz - xx, yz2 - tx2,
        zx2 - ty2, yz2 + tx2, tt + zz - xx - yy);
}

template<typename T>
inline CMat3x3<T> CQuat<T>::GetRevMat() const
{
    return GetRev().GetMat();
}

template<typename T>
inline CMat3x3<T> CQuat<T>::GetInvMat() const
{
    return GetInv().GetMat();
}

///////////////////////////////////////////////////////////////////////////////

template<typename T>
T CQuat<T>::ScalarProd(const CQuat<T> &q) const
{
    return m_t * q.m_t + m_x * q.m_x + m_y * q.m_y + m_z * q.m_z;
}

template<typename T>
T CQuat<T>::GetQuatAngleBetween(const CQuat<T> &q) const
{
    //CQuat<T> Quot = (GetInv() * q2);
    CQuat<T> Quot = (GetInv() * q);
    return Quot.GetQuatAngle();
}

template<typename T>
T CQuat<T>::GetRotAngleBetween(const CQuat<T> &q) const
{
    //CQuat<T> Quot = (GetInv() * q2);
    CQuat<T> Quot = (GetInv() * q);
    return Quot.GetRotAngle();
}

template<typename T>
CQuat<T> CQuat<T>::InterpLinear(const CQuat<T> &q, 
                                               T Frac) const
{
    const CQuat<T> &This = (*this);
    return This + (q - This) * Frac;
}

template<typename T>
CQuat<T> CQuat<T>::InterpStrictExp(const CQuat<T> &q, 
                                               T Frac) const
{
    const CQuat<T> qBase = (*this);
    CQuat<T> Quot = (qBase.GetInv() * q);
    CQuat<T> Delta = Quot.PowerTo(Frac);
    return qBase * Delta;
}

template<typename T>
CQuat<T> CQuat<T>::InterpShortestExp(const CQuat<T> &q, 
                                                 T Frac) const
{
    const CQuat<T> &qBase = (*this);
    CQuat<T> Quot = (qBase.GetInv() * q);
    CQuat<T> Delta = Quot.GetAbs().PowerTo(Frac);
    return qBase * Delta;
}

template<typename T>
CQuat<T> CQuat<T>::InterpStrictGeo(const CQuat<T> &q, 
                                                 T Frac) const
{
    const CQuat<T> qBase = (*this);
    float Scale1 = qBase.GetSqLen();
    float Scale2 = q.GetSqLen();
    float Scale = Scale1  + (Scale2 - Scale1) * Frac;
    float Fac = Sqrt(Scale / Scale1);
    if(!IsFinite(Fac))
    {
        // *this is near zero.
        return q * Fac; 
    }
    else
    {
        CQuat<T> Diff = (qBase.GetRev() * q);
        CQuat<T> Delta = Diff.NormalizedPowerTo(Frac);
        // If q is near zero, Delta is 1.
        return (qBase * Delta) * Fac;
    }
}

template<typename T>
CQuat<T> CQuat<T>::InterpShortestGeo(const CQuat<T> &q, 
                                                 T Frac) const
{
    const CQuat<T> qBase = (*this);
    float Scale1 = qBase.GetSqLen();
    float Scale2 = q.GetSqLen();
    float Scale = Scale1  + (Scale2 - Scale1) * Frac;
    float Fac = Sqrt(Scale / Scale1);
    if(!IsFinite(Fac))
    {
        // *this is near zero.
        return q * Fac;
    }
    else
    {
        CQuat<T> Diff = (qBase.GetRev() * q);
        CQuat<T> Delta = Diff.GetAbs().NormalizedPowerTo(Frac);
        // If q is near zero, Delta is 1.
        return (qBase * Delta) * Fac;
    }
}

template<typename T>
CQuat<T> CQuat<T>::GetProjAlong(
                                const CVec3<T> &Dir) const
{
    return CQuat<T>(m_t, GetVec().GetProjAlong(Dir));
}

template<typename T>
CQuat<T> CQuat<T>::GetProjOrthoLeft(
                                const CVec3<T> &Dir) const
{
    return (*this) * GetProjAlong(Dir).GetInv();
}

template<typename T>
CQuat<T> CQuat<T>::GetProjOrthoRight(
                                const CVec3<T> &Dir) const
{
    return GetProjAlong(Dir).GetInv() * (*this);
}

///////////////////////////////////////////////////////////////////////////////

template<typename T>
CQuat<T> CQuat<T>::ByRot(
                        const CVec3<T> &Axis, T Angle)
{
    T HalfAngle = 0.5 * Angle;
    T hs = Sin(HalfAngle);
    T hc = Cos(HalfAngle);
    T AxisLen = Axis.GetLen();
    T f = hs / AxisLen;
    if(ll3d::IsFinite(f))
        return CQuat<T>(hc, f * Axis.m_x, f * Axis.m_y, f * Axis.m_z);
    else
        return CQuat<T>(1);
}

template<typename T>
CQuat<T> CQuat<T>::ByScaledRot(
                        const CVec3<T> &Axis, T Angle, T Scale)
{
    T HalfAngle = 0.5 * Angle;
    T hs = Sin(HalfAngle);
    T hc = Cos(HalfAngle);
    T AxisLen = Axis.GetLen();
    T f = hs / AxisLen;
    if(IsFinite(f))
        return CQuat<T>(hc, f * Axis.m_x, f * Axis.m_y, f * Axis.m_z);
    else
        return CQuat<T>(AxisLen);
}

template<typename T>
CQuat<T> CQuat<T>::ByPathShortest(
                        const CVec3<T> &From, 
                        const CVec3<T> &To)
{
    CVec3<T> RezFrom = From.GetReziproc();
    CQuat<T> Base(RezFrom * To, RezFrom ^ To);
    return Base.GetSqRoot();
}

template<typename T>
CQuat<T> CQuat<T>::ByPathAxis(
                        const CVec3<T> &Axis,
                        const CVec3<T> &AlsoFrom, 
                        const CVec3<T> &AlsoTo)
{
    // Not using PathShortest(From x Axis, To x Axis) 
    // to avoid singularity at From = - To
    // Technique:
    // OrthoFrom := AlsoFrom.GetProjOrtho(Axis)
    // OrthoTo := AlsoFrom.GetProjOrtho(Axis)
    // sBase = sin(rotation_angle) = (OrthoFrom ^ OrthoTo) * UnitAxis
    // cBase = cos(rotation_angle) = OrthoFrom * OrthoTo
    // sHalf = sin(rotation_angle/2)
    // cHalf = cos(rotation_angle/2)
    CVec3<T> UnitAxis = Axis.GetNormalized();
    T AxisFrom = UnitAxis * AlsoFrom;
    T AxisTo = UnitAxis * AlsoTo;
    T sBase = UnitAxis.Volume(AlsoFrom, AlsoTo);
    T cBase = (AlsoFrom * AlsoTo) - AxisFrom * AxisTo;
    T BaseLen = Sqrt(cBase * cBase + sBase * sBase);
    T cUnit = cBase / BaseLen;
    T sHalf = Sqrt(0.5f * Abs(1 - cUnit)); // Abs for protection
    T cHalf = Sqrt(0.5f * Abs(1 + cUnit)); 
    if(sBase < 0) 
        sHalf = - sHalf;
    return CQuat<T>(cHalf, UnitAxis * sHalf);
}

template<typename T>
CQuat<T> CQuat<T>::ByPathAlso(
                        const CVec3<T> &From, 
                        const CVec3<T> &To,
                        const CVec3<T> &AlsoFrom, 
                        const CVec3<T> &AlsoTo)
{
    // First calculate shortest rotation:
    CQuat<T> Shortest = CQuat<T>::ByPathShortest(From, To);
    CVec3<T> EffAlsoFrom = Shortest.GetMat() * AlsoFrom;
    // Second, add extra rotation around To axis.
    CQuat<T> Extra = CQuat<T>::ByPathAxis(To, EffAlsoFrom, AlsoTo);
    return Extra * Shortest;
}

template<typename T>
CQuat<T> CQuat<T>::ByBestOfMat(
                        const CMat3x3<T> &m)
{
    // It uses the formulars 
    // SqRoot(0.5 * (m.GetTrace() - s), v = - 0.5 * m.GetDual())
    T s = Sqrt(m.GetSqLen() / 3);
    T qt = 0.5 * (m.m_xx + m.m_yy + m.m_zz - s);
    T qx = 0.5 * (m.m_zy - m.m_yz);            
    T qy = 0.5 * (m.m_xz - m.m_zx);
    T qz = 0.5 * (m.m_yx - m.m_xy);
    return CQuat<T>(qt, qx, qy, qz).GetSqRoot();
}

template<typename T>
CQuat<T> CQuat<T>::ByRotMat(
                        const CMat3x3<T> &m)
{
    T s = Sqrt(m.GetSqLen() / 3);
    T tt4 = s + m.m_xx + m.m_yy + m.m_zz;
    T xx4 = (s + m.m_xx) - (m.m_yy + m.m_zz);
    T yy4 = (s + m.m_yy) - (m.m_zz + m.m_xx);
    T zz4 = (s + m.m_zz) - (m.m_xx + m.m_yy);
    T t = tt4 > 0 ? 0.5 * Sqrt(tt4) : 0;
    T x = xx4 > 0 ? (m.m_yz <= m.m_zy ? 0.5 : -0.5) * Sqrt(xx4) : 0;
    T y = yy4 > 0 ? (m.m_zx <= m.m_xz ? 0.5 : -0.5) * Sqrt(yy4) : 0;
    T z = zz4 > 0 ? (m.m_xy <= m.m_yx ? 0.5 : -0.5) * Sqrt(zz4) : 0;
    return CQuat<T>(t, x, y, z);
}

template<typename T>
CQuat<T> CQuat<T>::ByRotXYZ(T Pan, T Tilt, T Roll)
{
    T HalfPan = 0.5 * Pan;
    T HalfTilt = 0.5 * Tilt;
    T HalfRoll = 0.5 * Roll;
    T SinX = Sin(HalfPan);
    T CosX = Cos(HalfPan);
    T SinY = Sin(HalfTilt);
    T CosY = Cos(HalfTilt);
    T SinZ = Sin(HalfRoll);
    T CosZ = Cos(HalfRoll);
    T SinXCosZ = SinX * CosZ;
    T SinXSinZ = SinX * SinZ;
    T CosXCosZ = CosX * CosZ;
    T CosXSinZ = CosX * SinZ;
    return CQuat<T>(
                CosXCosZ * CosY - SinXSinZ * SinY,
                SinXCosZ * CosY + CosXSinZ * SinY,
                CosXCosZ * SinY - SinXSinZ * CosY,
                CosXSinZ * CosY + SinXCosZ * SinY);
}

template<typename T>
void CQuat<T>::GetRotAngleXYZ(T &Pan, T &Tilt, T &Roll)
{
    T SinX = 2 * (m_t * m_x - m_y * m_z);
    T SinZ = 2 * (m_t * m_z - m_x * m_y);
    T SinY = 2 * (m_t * m_y + m_z * m_x);
    T CosU = m_t * m_t - m_y * m_y;
    T CosV = m_z * m_z - m_x * m_x;
    T CosX = CosU + CosV;
    T CosZ = CosU - CosV;
    T CosY = Sqrt(SinX * SinX + CosX * CosX);
    Pan = Atan2(SinX, CosX);
    Roll = Atan2(SinZ, CosZ);
    Tilt = Atan2(SinY, CosY);
}

///////////////////////////////////////////////////////////////////////////////

template<typename T>
inline CQuat<T> CQuat<T>::operator+(const CQuat<T> &q) const
{
    return CQuat<T>(m_t + q.m_t, m_x + q.m_x, m_y + q.m_y, m_z + q.m_z);
}

template<typename T>
inline CQuat<T> CQuat<T>::operator-(const CQuat<T> &q) const
{
    return CQuat<T>(m_t - q.m_t, m_x - q.m_x, m_y - q.m_y, m_z - q.m_z);
}

template<typename T>
inline CQuat<T> CQuat<T>::operator*(T s) const
{
    return CQuat<T>(m_t * s, m_x * s, m_y * s, m_z * s);
}

template<typename T>
inline CQuat<T> CQuat<T>::operator/(T s) const
{
    return CQuat<T>(m_t / s, m_x / s, m_y / s, m_z / s);
}

template<typename T>
CQuat<T> CQuat<T>::operator*(const CQuat<T> &q) const
{
    // == CQuat<T>(t1 * t2 - v1 * v2, t1 * v2 + v1 * t2 + v1 ^ v2);
    return CQuat<T>(
        m_t * q.m_t - m_x * q.m_x - m_y * q.m_y - m_z * q.m_z, 
        m_t * q.m_x + m_x * q.m_t + m_y * q.m_z - m_z * q.m_y, 
        m_t * q.m_y + m_y * q.m_t + m_z * q.m_x - m_x * q.m_z, 
        m_t * q.m_z + m_z * q.m_t + m_x * q.m_y - m_y * q.m_x);
}

template<typename T>
inline bool CQuat<T>::operator==(const CQuat<T> &q) const
{
    return m_t == q.m_t && m_x == q.m_x && m_y == q.m_y && m_z == q.m_z;
}

template<typename T>
inline bool CQuat<T>::operator!=(const CQuat<T> &q) const
{
    return m_t != q.m_t || m_x != q.m_x || m_y != q.m_y || m_z != q.m_z;
}

template<typename T>
int CQuat<T>::CompareComp(const CQuat<T> &q) const
{
    if(m_t < q.m_t)
        return -1;
    else if(m_t > q.m_t)
        return 1;
    else if(m_x < q.m_x)
        return -1;
    else if(m_x > q.m_x)
        return 1;
    else if(m_y < q.m_y)
        return -1;
    else if(m_y > q.m_y)
        return 1;
    else if(m_z < q.m_z)
        return -1;
    else if(m_z > q.m_z)
        return 1;
    else
        return 0;
}


}//namespace ll3d
#endif //_LL3D_VEC3_H