fEulerPara.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * The University of Tokyo
 */
#ifndef __FEULERPARA_H_
#define __FEULERPARA_H_

#include "hrpModelExportDef.h"
#include "fMatrix4.h"

class HRPBASE_EXPORT fEulerPara 
        : public fVec4
{
public:
        fEulerPara() : fVec4() {
        }
        ~fEulerPara() {
        }
        fEulerPara(const fVec3& v, double s) {
                set(v, s);
        }
        fEulerPara(double s1, double s2, double s3, double s4) {
                set(s1, s2, s3, s4);
        }
        
        friend double& Ang(fEulerPara& ep) {
                return ep.m_scalar;
        }
        double& Ang() {
                return m_scalar;
        }
        double Ang() const {
                return m_scalar;
        }
        friend fVec3& Axis(fEulerPara& ep) {
                return ep.m_vec;
        }
        fVec3& Axis() {
                return m_vec;
        }
        const fVec3& Axis() const {
                return m_vec;
        }
        fEulerPara operator= (const fEulerPara&);
        fEulerPara operator= (const fMat33&);
        void set(const fVec3& v, double s) {
                m_scalar = s;
                m_vec = v;
        }
        void set(double s1, double s2, double s3, double s4) {
                m_vec.set(s1, s2, s3);
                m_scalar = s4;
        }
        void set(const fEulerPara&);
        void set(const class fMat33&);

        void identity() {
                m_scalar = 1.0;
                m_vec.zero();
        }
        void unit();
        friend fEulerPara unit(const fEulerPara&);

        friend fEulerPara operator * (double, const fEulerPara&);
        friend fEulerPara operator - (const fEulerPara&);
        friend fEulerPara mat2ep(const fMat33&);
        friend fMat33 ep2mat(const fEulerPara&);


        void angvel2epdot(const fEulerPara& _ep, const fVec3& _omega);
        friend fEulerPara angvel2epdot(const fEulerPara& epara, const fVec3& vel);
        friend fVec3 epdot2angvel(const fEulerPara& epara, const fEulerPara& edot);

        friend fVec3 epddot2angacc(const fEulerPara& e, const fEulerPara& de, const fEulerPara& dde);

        void interpolate(const fEulerPara& ep1, const fEulerPara& ep2, double t);

        double rotation(const fEulerPara& ep0, const fVec3& s);

};

#endif