vpMomentCentered.cpp

/****************************************************************************
 *
 * $Id: vpMomentCentered.cpp 4712 2014-03-28 17:55:43Z mbakthav $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Centered moment descriptor
 *
 * Authors:
 * Filip Novotny
 *
 *****************************************************************************/

#include <visp/vpMomentCentered.h>
#include <visp/vpMomentObject.h>
#include <visp/vpMomentGravityCenter.h>
#include <exception>
#include <cassert>

void vpMomentCentered::set(unsigned int i, unsigned int j, double value){
    vpMomentObject mobj = getObject();
    assert(i+j<=mobj.getOrder());
    if(i+j>mobj.getOrder()) throw vpException(vpException::badValue,"You cannot set that value.");
    values[j*(mobj.getOrder()+1)+i] = value;
}

void vpMomentCentered::compute(){    
    bool found_moment_gravity;    
    values.resize((getObject().getOrder()+1)*(getObject().getOrder()+1));

    const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
    if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");

    unsigned int order = getObject().getOrder()+1;
    for(register unsigned int j=0;j<(order);j++){
        for(register unsigned int i=0;i<order-j;i++){
            unsigned int c = order*j+i;
            values[c]=0;
            for(register unsigned int k=0;k<=i;k++){
                double Xg_i_k = pow(-momentGravity.get()[0],(int)(i-k));
                double comb_i_k = static_cast<double>( vpMath::comb(i,k) );
                for(register unsigned int l=0;l<=j;l++){
                    values[c]+= static_cast<double>( comb_i_k*vpMath::comb(j,l)
                      *Xg_i_k
                      *pow(-momentGravity.get()[1],(int)(j-l))*getObject().get(k,l) );
                }
            }
        }
    }

}

vpMomentCentered::vpMomentCentered() : vpMoment(){

}

double vpMomentCentered::get(unsigned int i,unsigned int j) const {
    unsigned int order = getObject().getOrder();
    assert(i+j<=order);
    if(i+j>order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");

    return values[j*(order+1)+i];
}

VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCentered& m){
    for(unsigned int i = 0;i<m.values.size();i++){
        if(i%(m.getObject().getOrder()+1)==0)
          os << std::endl;

        if((i%(m.getObject().getOrder()+1)+i/(m.getObject().getOrder()+1))<m.getObject().getOrder()+1)
            os << m.values[i] ;
        else
            os << "x";

        os << "\t";
    }
    os << std::endl;
    m.printWithIndices(os);
    return os;
}

void
vpMomentCentered::printWithIndices(std::ostream& os) const {
    unsigned int orderp1 = getObject().getOrder()+1;
    for(unsigned int k=0; k<orderp1; k++) {
        for(unsigned int l=0; l<orderp1-k; l++)
        {
            os << "mu[" << k << "," << l << "] = " << this->get(k,l) << "\t";
        }
        os << std::endl;
    }
    os << std::endl;
}

void
vpMomentCentered::printDependencies(std::ostream& os) const {
    os << (__FILE__) << std::endl;
    /*
    Retreive the raw moments
    */
    const vpMomentObject objt = getObject();
    vpMomentObject::printWithIndices(objt, os);

    /*
    Get xg,yg
    */
    bool found_moment_gravity;
    const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
    if(!found_moment_gravity)
        throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
    os << "Xg = " << momentGravity.getXg() << "\t" << "Yg = " << momentGravity.getYg() << std::endl;
}