/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */

/*
  $Id: vctMatrixRotation3Base.cpp,v 1.13 2007/04/26 19:33:57 anton Exp $

  Author(s):	Anton Deguet
  Created on:	2005-08-20

  (C) Copyright 2005-2007 Johns Hopkins University (JHU), All Rights
  Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---

*/


#include <cisstVector/vctMatrixRotation3Base.h>

template <>
const vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> > &
vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> >::Identity()
{
    static const ThisType result(1.0, 0.0, 0.0,
                                 0.0, 1.0, 0.0,
                                 0.0, 0.0, 1.0);
    return result;
}

template <>
const vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> > &
vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> >::Identity()
{
    static const ThisType result(1.0f, 0.0f, 0.0f,
                                 0.0f, 1.0f, 0.0f,
                                 0.0f, 0.0f, 1.0f);
    return result;
}



template <class _containerType>
vctMatrixRotation3Base<_containerType> &
vctMatrixRotation3Base<_containerType>::FromRaw(const vctAxisAngleRotation3<typename _containerType::value_type> & axisAngleRotation) {

    typedef vctAxisAngleRotation3<value_type> AxisAngleType;

    const typename AxisAngleType::AngleType angle = axisAngleRotation.Angle();
    const typename AxisAngleType::AxisType axis = axisAngleRotation.Axis();
    
    const AngleType sinAngle = AngleType(sin(angle));
    const AngleType cosAngle = AngleType(cos(angle));
    const AngleType CompCosAngle = 1 - cosAngle;
    
    // first column
    this->Element(0, 0) = value_type(axis[0] * axis[0] * CompCosAngle + cosAngle);
    this->Element(1, 0) = value_type(axis[0] * axis[1] * CompCosAngle + axis[2] * sinAngle);
    this->Element(2, 0) = value_type(axis[0] * axis[2] * CompCosAngle - axis[1] * sinAngle);
    
    // second column
    this->Element(0, 1) = value_type(axis[1] * axis[0] * CompCosAngle - axis[2] * sinAngle);
    this->Element(1, 1) = value_type(axis[1] * axis[1] * CompCosAngle + cosAngle);
    this->Element(2, 1) = value_type(axis[1] * axis[2] * CompCosAngle + axis[0] * sinAngle);
    
    // third column
    this->Element(0, 2) = value_type(axis[2] * axis[0] * CompCosAngle + axis[1] * sinAngle);
    this->Element(1, 2) = value_type(axis[2] * axis[1] * CompCosAngle - axis[0] * sinAngle);
    this->Element(2, 2) = value_type(axis[2] * axis[2] * CompCosAngle + cosAngle);
    
    return *this;
}




template <class _matrixType, class _quaternionType>
void
vctMatrixRotation3BaseFromRaw(vctMatrixRotation3Base<_matrixType> & matrixRotation,
                              const vctQuaternionRotation3Base<_quaternionType> & quaternionRotation) {

    typedef typename _matrixType::value_type value_type;

    value_type xx = quaternionRotation.X() * quaternionRotation.X();
    value_type xy = quaternionRotation.X() * quaternionRotation.Y();
    value_type xz = quaternionRotation.X() * quaternionRotation.Z();
    value_type xr = quaternionRotation.X() * quaternionRotation.R();
    value_type yy = quaternionRotation.Y() * quaternionRotation.Y();
    value_type yz = quaternionRotation.Y() * quaternionRotation.Z();
    value_type yr = quaternionRotation.Y() * quaternionRotation.R();
    value_type zz = quaternionRotation.Z() * quaternionRotation.Z();
    value_type zr = quaternionRotation.Z() * quaternionRotation.R();
    matrixRotation.Assign(1 - 2 * (yy + zz), 2 * (xy - zr),     2 * (xz + yr),
                          2 * (xy + zr),     1 - 2 * (xx + zz), 2 * (yz - xr),
                          2 * (xz - yr),     2 * (yz + xr),     1 - 2 * (xx + yy));
}



// specialize for fixed size matrices
template <>
vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> > &
vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> >::NormalizedSelf(void) {
    vctQuaternionRotation3Base<vctFixedSizeVector<double, 4> > quaternion;
    quaternion.FromRaw(*this);
    quaternion.NormalizedSelf();
    this->From(quaternion);
    return *this;
}

template <>
vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> > &
vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> >::NormalizedSelf(void) {
    vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> > quaternion;
    quaternion.FromRaw(*this);
    quaternion.NormalizedSelf();
    this->From(quaternion);
    return *this;
}


// force the instantiation of the templated classes
template class vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> >;
template class vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> >;

template <>
const vctMatrixRotation3Base<vctDynamicMatrix<double> > &
vctMatrixRotation3Base<vctDynamicMatrix<double> >::Identity()
{
    static const ThisType result(1.0, 0.0, 0.0,
                                 0.0, 1.0, 0.0,
                                 0.0, 0.0, 1.0);
    return result;
}


template <>
vctMatrixRotation3Base<vctDynamicMatrix<double> > &
vctMatrixRotation3Base<vctDynamicMatrix<double> >::NormalizedSelf(void) {

    vctQuaternionRotation3Base<vctDynamicVector<double> > quaternion;
    quaternion.FromRaw(*this);
    quaternion.NormalizedSelf();
    this->From(quaternion);
    return *this;
}

template <>
const vctMatrixRotation3Base<vctDynamicMatrix<float> > &
vctMatrixRotation3Base<vctDynamicMatrix<float> >::Identity()
{
    static const ThisType result(1.0, 0.0, 0.0,
                                 0.0, 1.0, 0.0,
                                 0.0, 0.0, 1.0);
    return result;
}


template <>
vctMatrixRotation3Base<vctDynamicMatrix<float> > &
vctMatrixRotation3Base<vctDynamicMatrix<float> >::NormalizedSelf(void) {

    vctQuaternionRotation3Base<vctDynamicVector<float> > quaternion;
    quaternion.FromRaw(*this);
    quaternion.NormalizedSelf();
    this->From(quaternion);
    return *this;
}


// force instantiation of helper functions
template void
vctMatrixRotation3BaseFromRaw(vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> > & matrixRotation,
                              const vctQuaternionRotation3Base<vctFixedSizeVector<double, 4> > & quaternionRotation);
template void
vctMatrixRotation3BaseFromRaw(vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> > & matrixRotation,
                              const vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> > & quaternionRotation);


// force instantiation for dynamic containers, this is used for Python wrappers
template void
vctMatrixRotation3BaseFromRaw(vctMatrixRotation3Base<vctDynamicMatrix<double> > & matrixRotation,
                              const vctQuaternionRotation3Base<vctDynamicVector<double> > & quaternionRotation);

template void
vctMatrixRotation3BaseFromRaw(vctMatrixRotation3Base<vctDynamicMatrix<float> > & matrixRotation,
                              const vctQuaternionRotation3Base<vctDynamicVector<float> > & quaternionRotation);

template void
vctMatrixRotation3BaseFromRaw(vctMatrixRotation3Base<vctDynamicMatrix<double> > & matrixRotation,
                              const vctQuaternionRotation3Base<vctFixedSizeVector<double, 4> > & quaternionRotation);

template void
vctMatrixRotation3BaseFromRaw(vctMatrixRotation3Base<vctDynamicMatrix<float> > & matrixRotation,
                              const vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> > & quaternionRotation);


template class vctMatrixRotation3Base<vctDynamicMatrix<double> >;
template class vctMatrixRotation3Base<vctDynamicMatrix<float> >;


// ****************************************************************************
//                              Change History
// ****************************************************************************
//
// $Log: vctMatrixRotation3Base.cpp,v $
// Revision 1.13  2007/04/26 19:33:57  anton
// All files in libraries: Applied new license text, separate copyright and
// updated dates, added standard header where missing.
//
// Revision 1.12  2007/02/27 17:25:26  anton
// cisstVector transformations: Force instantiation of classes and functions to
// support transformations based on dynamic containers.
//
// Revision 1.11  2007/02/27 15:52:46  anton
// cisstVector transformations: Cleanup code related to template instantiation
// for transformation based on dynamic containers and added support for floats.
//
// Revision 1.10  2007/01/23 20:59:27  anton
// cisstVector: Updated transformations classes so that non const methods
// returning a reference on "this" return a non const reference.  This follows
// ticket #259.
//
// Revision 1.9  2006/11/20 20:33:19  anton
// Licensing: Applied new license to cisstCommon, cisstVector, cisstNumerical,
// cisstInteractive, cisstImage and cisstOSAbstraction.
//
// Revision 1.8  2005/11/17 22:48:50  anton
// cisstVector transformations: Added required explicit casts to avoid warnings
// with Visual Studio compiler.  See checkin [1528] and ticket #192.
//
// Revision 1.7  2005/11/15 03:24:29  anton
// cisstVector transformations:  Use AngleType and NormType instead of
// value_type for local variables to increase numerical stability.
//
// Revision 1.6  2005/10/10 01:13:13  anton
// vctMatrixRotation3Base: Instantiations didn't match declaration (throw)
//
// Revision 1.5  2005/10/10 00:53:39  anton
// vctMatrixRotation3Base: Specialize the ctor from 9 elements based on the
// containerType.  This allows to use this ctor in Identity() and create a true
// static variable (see #179).
//
// Revision 1.4  2005/10/07 03:26:31  anton
// vctMatrixRotation3Base.cpp: Modified implementation of Identity() for dynamic
// matrices to avoid recusrsive call of default constructor.
//
// Revision 1.3  2005/09/26 15:41:46  anton
// cisst: Added modelines for emacs and vi.
//
// Revision 1.2  2005/09/07 03:29:48  anton
// vct{matrix,Quaternion}Rotation3Base.cpp: Changed order of instantiation for
// gcc 4.0 (worked with gcc 3.3 and 3.4).
//
// Revision 1.1  2005/09/01 06:18:08  anton
// cisstVector transformations: Added a level of abstraction for all classes
// derived from fixed size containers (vctQuaternion, vctQuaternionRotation3,
// vctMatrixRotation3, vctRodriguezRotation3).  These classes are now derived
// from vctXyzBase<_containerType>.  This will ease the SWIG wrapping.
//
//
// ****************************************************************************