/* -*- 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: vctQuaternionRotation3Base.cpp,v 1.10 2007/04/26 19:33:57 anton Exp $
  
  Author(s):	Anton Deguet
  Created on:	2005-08-24

  (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/vctFixedSizeMatrix.h>
#include <cisstVector/vctDynamicMatrix.h>
#include <cisstVector/vctQuaternionRotation3Base.h>
#include <cisstVector/vctMatrixRotation3Base.h>

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

template<>
const vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> > &
vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> >::Identity()
{
    static const ThisType result(0.0f, 0.0f, 0.0f, 1.0f);
    return result;
}


template <class _quaternionType, class _matrixType>
void
vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base<_quaternionType> & quaternionRotation,
                                  const vctMatrixRotation3Base<_matrixType> & matrixRotation)
{
    typedef typename _quaternionType::value_type value_type;
    typedef typename _quaternionType::TypeTraits TypeTraits;
    typedef typename _quaternionType::NormType NormType;

    NormType trace, traceInverse;
    trace = NormType(1.0) + matrixRotation.Element(0, 0) + matrixRotation.Element(1, 1) + matrixRotation.Element(2, 2);
    if (vctUnaryOperations<NormType>::AbsValue::Operate(trace) > cmnTypeTraits<NormType>::Tolerance()) {
        trace = NormType(sqrt(trace) * 2.0);
        traceInverse = NormType(1.0) / trace;
        quaternionRotation.X() = value_type((matrixRotation.Element(2, 1) - matrixRotation.Element(1, 2)) * traceInverse);
        quaternionRotation.Y() = value_type((matrixRotation.Element(0, 2) - matrixRotation.Element(2, 0)) * traceInverse);
        quaternionRotation.Z() = value_type((matrixRotation.Element(1, 0) - matrixRotation.Element(0, 1)) * traceInverse);
        quaternionRotation.R() = value_type(0.25 * trace);
    } else 
        if (matrixRotation.Element(0, 0) > matrixRotation.Element(1, 1) && matrixRotation.Element(0, 0) > matrixRotation.Element(2, 2))  {
            trace  = NormType(sqrt(1.0 + matrixRotation.Element(0, 0) - matrixRotation.Element(1, 1) - matrixRotation.Element(2, 2)) * 2.0);
            traceInverse = NormType(1.0) / trace;
            quaternionRotation.X() = value_type(0.25 * trace);
            quaternionRotation.Y() = value_type((matrixRotation.Element(0, 1) + matrixRotation.Element(1, 0)) * traceInverse);
            quaternionRotation.Z() = value_type((matrixRotation.Element(2, 0) + matrixRotation.Element(0, 2)) * traceInverse);
            quaternionRotation.R() = value_type((matrixRotation.Element(1, 2) - matrixRotation.Element(2, 1)) * traceInverse);
        } else if (matrixRotation.Element(1, 1) > matrixRotation.Element(2, 2)) {
            trace  = NormType(sqrt(1.0 + matrixRotation.Element(1, 1) - matrixRotation.Element(0, 0) - matrixRotation.Element(2, 2)) * 2.0);
            traceInverse = NormType(1.0) / trace;
            quaternionRotation.X() = value_type((matrixRotation.Element(0, 1) + matrixRotation.Element(1, 0)) * traceInverse);
            quaternionRotation.Y() = value_type(0.25 * trace);
            quaternionRotation.Z() = value_type((matrixRotation.Element(1, 2) + matrixRotation.Element(2, 1)) * traceInverse);
            quaternionRotation.R() = value_type((matrixRotation.Element(2, 0) - matrixRotation.Element(0, 2)) * traceInverse);
        } else {
            trace  = NormType(sqrt(1.0 + matrixRotation.Element(2, 2) - matrixRotation.Element(0, 0) - matrixRotation.Element(1, 1)) * 2.0);
            traceInverse = NormType(1.0) / trace;
            quaternionRotation.X() = value_type((matrixRotation.Element(2, 0) + matrixRotation.Element(0, 2)) * traceInverse);
            quaternionRotation.Y() = value_type((matrixRotation.Element(1, 2) + matrixRotation.Element(2, 1)) * traceInverse);
            quaternionRotation.Z() = value_type(0.25 * trace);
            quaternionRotation.R() = value_type((matrixRotation.Element(0, 1) - matrixRotation.Element(1, 0)) * traceInverse);
        }
}


// force the instantiation of the templated classes
template class vctQuaternionRotation3Base<vctFixedSizeVector<double, 4> >;
template class vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> >;

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

// force instantiation for dynamic containers, this is used for Python wrappers
template <>
const vctQuaternionRotation3Base<vctDynamicVector<double> > &
vctQuaternionRotation3Base<vctDynamicVector<double> >::Identity()
{
    static const ThisType result(0.0, 0.0, 0.0, 1.0);
    return result;
}

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

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

template class vctQuaternionRotation3Base<vctDynamicVector<double> >;
template class vctQuaternionRotation3Base<vctDynamicVector<float> >;



// fixed / dynamic
template void
vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base<vctFixedSizeVector<double, 4> > & quaternionRotation,
                                  const vctMatrixRotation3Base<vctDynamicMatrix<double> > & matrixRotation);
template void
vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> > & quaternionRotation,
                                  const vctMatrixRotation3Base<vctDynamicMatrix<float> > & matrixRotation);

// dynamic / fixed
template void
vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base<vctDynamicVector<double> > & quaternionRotation,
                                  const vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> > & matrixRotation);
template void
vctQuaternionRotation3BaseFromRaw(vctQuaternionRotation3Base<vctDynamicVector<float> > & quaternionRotation,
                                  const vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> > & matrixRotation);


// ****************************************************************************
//                              Change History
// ****************************************************************************
//
// $Log: vctQuaternionRotation3Base.cpp,v $
// Revision 1.10  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.9  2007/02/27 17:25:26  anton
// cisstVector transformations: Force instantiation of classes and functions to
// support transformations based on dynamic containers.
//
// Revision 1.8  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.7  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.6  2006/11/20 20:33:19  anton
// Licensing: Applied new license to cisstCommon, cisstVector, cisstNumerical,
// cisstInteractive, cisstImage and cisstOSAbstraction.
//
// Revision 1.5  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.4  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.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.
//
//
// ****************************************************************************