/* -*- 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: vctAxisAngleRotation3.cpp,v 1.15 2007/04/26 19:33:57 anton Exp $
  
  Author(s):	Anton Deguet
  Created on:	2004-01-13

  (C) Copyright 2004-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 <cisstCommon/cmnConstants.h>
#include <cisstVector/vctAxisAngleRotation3.h>
#include <cisstVector/vctQuaternionRotation3Base.h>
#include <cisstVector/vctMatrixRotation3Base.h>

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


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


template <class _elementType, class _containerType>
void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<_elementType> & axisAngleRotation,
                             const vctQuaternionRotation3Base<_containerType> & quaternionRotation) {
    const _elementType r = quaternionRotation.R();
    axisAngleRotation.Angle() = acos(r) * 2.0;
    double oneMinusR2 = 1.0 - r * r;
    if (oneMinusR2 < cmnTypeTraits<_elementType>::Tolerance())
        oneMinusR2 = 0.0;
    _elementType sinAngle = _elementType(sqrt(oneMinusR2));
    if (vctUnaryOperations<_elementType>::AbsValue::Operate(sinAngle) > cmnTypeTraits<_elementType>::Tolerance()) {
        axisAngleRotation.Axis().X() = quaternionRotation.X() / sinAngle;
        axisAngleRotation.Axis().Y() = quaternionRotation.Y() / sinAngle;
        axisAngleRotation.Axis().Z() = quaternionRotation.Z() / sinAngle;
    } else {
        axisAngleRotation.Axis().X() = quaternionRotation.X();
        axisAngleRotation.Axis().Y() = quaternionRotation.Y();
        axisAngleRotation.Axis().Z() = quaternionRotation.Z();
    }
}


template <class _elementType, class _containerType>
void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<_elementType> & axisAngleRotation,
                             const vctMatrixRotation3Base<_containerType> & matrixRotation) {

    typedef vctAxisAngleRotation3<_elementType> AxisAngleRotationType;
    typedef typename AxisAngleRotationType::NormType NormType;
    typedef typename AxisAngleRotationType::AngleType AngleType;

    const NormType normTolerance = cmnTypeTraits<NormType>::Tolerance();
    const NormType trace = matrixRotation.Element(0, 0) + matrixRotation.Element(1, 1) + matrixRotation.Element(2, 2); // 2 * cos(angle) + 1
    const NormType xSin = matrixRotation.Element(2, 1) - matrixRotation.Element(1, 2); // 2 * x * sin(angle)
    const NormType ySin = matrixRotation.Element(0, 2) - matrixRotation.Element(2, 0); // 2 * y * sin(angle)
    const NormType zSin = matrixRotation.Element(1, 0) - matrixRotation.Element(0, 1); // 2 * z * sin(angle)
    const NormType normSquare = xSin * xSin + ySin * ySin + zSin * zSin; 
    NormType norm;
    if (normSquare < normTolerance) {
        norm = 0.0;
    } else {
        norm = sqrt(normSquare); // 2 * |sin(angle)|
    }
    
    // either 0 or PI
    if (norm == 0.0) {
        const NormType traceMinus3 = trace - NormType(3);
        // if the angle is 0, then cos(angle) = 1, and trace = 2*cos(angle) + 1 = 3
        if ( (traceMinus3 > -normTolerance) && (traceMinus3 < normTolerance) ) {
            axisAngleRotation.Angle() = _elementType(0);
            axisAngleRotation.Axis().Assign(_elementType(0), _elementType(0), _elementType(1));
            return;
        }
        // since norm is already 0, we are in the other case, i.e., angle-PI, but we just want
        // to assert that trace = -1
        CMN_ASSERT( (trace > (NormType(-1) - normTolerance)) && (trace < (NormType(-1) + normTolerance)) );
        // the diagonal is [k_x*k_x*v + c ,  k_y*k_y*v + c,  k_z*k_z*v + c]
        // c = -1 ;  v = (1 - c) = 2
        NormType xSquare = (matrixRotation.Element(0, 0) + 1) / 2;
        NormType ySquare = (matrixRotation.Element(1, 1) + 1) / 2;
        NormType zSquare = (matrixRotation.Element(2, 2) + 1) / 2;
        if (xSquare < normTolerance)
            xSquare = 0;
        if (ySquare < normTolerance)
            ySquare = 0;
        if (zSquare < normTolerance)
            zSquare = 0;
        NormType x = sqrt(xSquare);
        NormType y = sqrt(ySquare);
        NormType z = sqrt(zSquare);
        // we arbitrarily decide the k_x is positive, if it's zero then k_y is positive, and if both are zero, then k_z is positive
        if (x > 0) {
            if (matrixRotation.Element(1, 0) < 0) // Element(1,0) = k_x*k_y*v , where v=2, so we just need to check its sign
                y = -y;
            if (matrixRotation.Element(2, 0) < 0) // Element(2,0) = k_x*k_z*v
                z = -z;
        } else if (y > 0) {
            if (matrixRotation.Element(2, 1) < 0) // Element(2,1) = k_y*k_z*v
                z = -z;
        } else {
            z = 1.0; // x and y are zero, Z has to be one
        }
        axisAngleRotation.Axis().Assign(_elementType(x), _elementType(y), _elementType(z));
        axisAngleRotation.Angle() = _elementType(cmnPI);
        return;
    }
    
    const AngleType angle = atan2(norm / 2, (trace - 1) / 2);
    axisAngleRotation.Axis().Assign(_elementType(xSin), _elementType(ySin), _elementType(zSin));
    axisAngleRotation.Axis().NormalizedSelf();
    axisAngleRotation.Angle() = _elementType(angle);
}


template <class _elementType, class _containerType>
void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<_elementType> & axisAngleRotation,
                             const vctRodriguezRotation3Base<_containerType> & rodriguezRotation) {
    
    typedef vctAxisAngleRotation3<_elementType> AxisAngleRotationType;
    typedef typename AxisAngleRotationType::value_type value_type;
    typedef typename AxisAngleRotationType::NormType NormType;
    typedef typename AxisAngleRotationType::AngleType AngleType;

    const NormType axisLength = rodriguezRotation.Norm();
    const value_type axisTolerance = cmnTypeTraits<value_type>::Tolerance();
    const AngleType angle = (axisLength < axisTolerance) ? 0.0 : axisLength;
    axisAngleRotation.Angle() = angle;
    if (angle == 0.0) {
        axisAngleRotation.Axis().Assign(value_type(1), value_type(0), value_type(0));
    } else {
        axisAngleRotation.Axis().Assign(rodriguezRotation.X() / static_cast<value_type>(axisLength),
                                        rodriguezRotation.Y() / static_cast<value_type>(axisLength),
                                        rodriguezRotation.Z() / static_cast<value_type>(axisLength));
    }
}


// force the instantiation of the templated classes
template class vctAxisAngleRotation3<double>;
template class vctAxisAngleRotation3<float>;


// force instantiation of helper functions
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctQuaternionRotation3Base<vctFixedSizeVector<double, 4> > & quaternionRotation);
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctQuaternionRotation3Base<vctFixedSizeVector<float, 4> > & quaternionRotation);

template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctMatrixRotation3Base<vctFixedSizeMatrix<double, 3, 3> > & matrixRotation);
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctMatrixRotation3Base<vctFixedSizeMatrix<float, 3, 3> > & matrixRotation);

template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctRodriguezRotation3Base<vctFixedSizeVector<double, 3> > & rodriguezRotation);
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctRodriguezRotation3Base<vctFixedSizeVector<float, 3> > & rodriguezRotation);


// force instantiation for dynamic containers, this is used for Python wrappers
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctQuaternionRotation3Base<vctDynamicVector<double> > & quaternionRotation);
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctQuaternionRotation3Base<vctDynamicVector<float> > & quaternionRotation);

template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctMatrixRotation3Base<vctDynamicMatrix<double> > & matrixRotation);
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctMatrixRotation3Base<vctDynamicMatrix<float> > & matrixRotation);

template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<double> & axisAngleRotation,
                             const vctRodriguezRotation3Base<vctDynamicVector<double> > & rodriguezRotation);
template void
vctAxisAngleRotation3FromRaw(vctAxisAngleRotation3<float> & axisAngleRotation,
                             const vctRodriguezRotation3Base<vctDynamicVector<float> > & rodriguezRotation);


// ****************************************************************************
//                              Change History
// ****************************************************************************
//
// $Log: vctAxisAngleRotation3.cpp,v $
// Revision 1.15  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.14  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.13  2007/01/29 03:29:37  anton
// cisstVector: Update quaternion and quaternion based rotation to reflect new
// class structure (see ticket #263).
//
// Revision 1.12  2007/01/26 21:23:22  anton
// cisstVector:  Updated vctMatrixRotation3 code to reflect new design (i.e. avoid
// derived class and use typedef instead, see ticket #263.
//
// Revision 1.11  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.10  2006/11/20 20:33:19  anton
// Licensing: Applied new license to cisstCommon, cisstVector, cisstNumerical,
// cisstInteractive, cisstImage and cisstOSAbstraction.
//
// Revision 1.9  2006/03/27 13:52:36  kapoor
// vctAxisAngleRotation: changed plain assert to CMN_ASSERT, in method to convert
// rotation to axis angle. This method must be revisited and fixed as it does *NOT*
// handle all cases.
//
// Revision 1.8  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.7  2005/10/17 21:59:57  anton
// cisst: Introduced cmnPI and co.  Updated whole repository. See #131.
//
// Revision 1.6  2005/09/26 15:41:46  anton
// cisst: Added modelines for emacs and vi.
//
// Revision 1.5  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.
//
// Revision 1.4  2005/05/19 19:29:01  anton
// cisst libs: Added the license to cisstCommon and cisstVector
//
// Revision 1.3  2005/02/24 03:37:01  anton
// vctAxisAngleRotation3: New code for From(vctMatrixRotation3) based on
// algorithm developed by Ofri for vctRodriguezRotation3::From(vctMatrixRotation3).
//
// Revision 1.2  2005/02/08 22:09:15  ofri
// Following ticket #127, I added some safety checks in conversions to
// vctAxisAngleRotation3 and vctRodriguezRotation3 From other types.
// It is still necessary to go over all the evaluations of sqrt in our code and make
// sure that it doesn't take negative input.
//
// Revision 1.1  2005/02/04 16:56:54  anton
// cisstVector: Added classes vctAxisAngleRotation3 and vctRodriguezRotation3
// as planed in ticket #109.  Also updated vctMatrixRotation3 to use these new
// classes.  Other updates include vctRandom(rotation), new vctTypes and
// forward declarations.
//
//
// ****************************************************************************