Rotation.h

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #ifndef __ROTATION_H__
00020 #define __ROTATION_H__
00021 
00022 #include "mathlib.h"
00023 #include <assert.h>
00024 
00025 
00026 
00033 class Rotation {
00034   
00035 protected:
00036   
00037     CVector3_t beta; 
00038     float alpha; 
00041     mutable CVector3_t axis; 
00042     mutable float norm;  //sin(theta/2)
00046     mutable int axis_flag;
00047 
00048 
00049     float eps_b; 
00050     float eps_a; 
00052     float base_eps_b;
00053     float base_eps_a;
00054 
00055 
00060     void ComputeMatrixC(const CVector3_t v, CMatrix3_t out)const ;
00061 
00067     void ComputeMatrixB(const CVector3_t v, CMatrix3_t out)const;
00068 
00069 
00070 
00071 public:
00072     Rotation();
00073     virtual ~Rotation();
00074     int GetRotationAxis(CVector3_t axis)const;
00075     const float* GetRotationAxis()const{if(!AxisOk())SetAxis(); return axis;};
00076     virtual int SetTransfo(CVector3_t beta);
00077     void GetRotationParam(CVector3_t param)const{v_copy(beta,param);};
00078     const float *GetRotationParam()const{return beta;}
00079     void GetQuaternion(CQuat_t q)const {v_copy(beta,q);q[3]=alpha;};
00080     void GetInverseQuaternion(CQuat_t q)const {v_scale(beta,-1,q);q[3]=alpha;};
00081     void SetQuaternion(CQuat_t q);
00082     float GetRotationAngle()const;
00083     void Derivative(CMatrix3_t out) const;
00084     int SetAxis()const {norm = v_normalize(beta,axis);return axis_flag = (norm>epsilon);}
00085     int AxisOk()const{return axis_flag;}
00086     void CheckAxis()const{if(!AxisOk())SetAxis();}
00087     
00088     
00089     
00093     void InvertRotationAngle(){v_scale(beta,-1,beta);norm = -norm;}
00094     
00095 
00105     float AimingAngle(const CVector3_t tar, const CVector3_t with);
00106     
00107     
00117     void AimAt(const CVector3_t tar, const CVector3_t with){SetAngle(AimingAngle(tar,with));};
00118     
00119     
00125     void AngleDerivative(CVector3_t v, CVector3_t out);
00126     
00132     void NormDerivative(CVector3_t v, CVector3_t out);
00133  
00139     void InverseNormDerivative(CVector3_t v, CVector3_t out);
00145     void BetaDerivative(CVector3_t x, CMatrix3_t out)const;
00146 
00152     void AxisDerivative(CVector3_t x, CMatrix3_t out);
00153  
00159     void InverseAxisDerivative(CVector3_t v, CMatrix3_t out);
00160   
00161     void AngleAxisDerivative(CVector3_t v, CMatrix3_t out);
00162  
00163     void Jacobian(CMatrix3_t out)const;
00164 
00165 
00169     void Add(CVector3_t dbeta);
00170 
00174     void AddToAxis(CVector3_t daxis);
00179     void AddToNorm(float dnorm);
00180 
00185     void  RotFromMatrix(const CMatrix3_t m);
00186 
00191     void  RotToMatrix(CMatrix3_t m)const;//{CQuat_t q;GetQuaternion(q);q_to_matrix(q,m);}
00192 
00199     void Copy(const Rotation& r);
00200 
00210     void Update(CVector3_t v, CVector3_t v_tr);
00211 
00212 
00213     void UpdateAxis(CVector3_t v, CVector3_t v_tr,float angle);
00214 
00220     virtual void SetRate(float new_eps);
00221 
00227     virtual void ScaleRate(float factor);
00228 
00234     void Transform(const CVector3_t in, CVector3_t out)const;
00235 
00241     void InverseTransform(const CVector3_t in, CVector3_t out);
00242 
00243 
00247     void TransformRotation(const CVector3_t beta_in, CVector3_t beta_out);
00248 
00253     void QuaternionNormDerivative(CQuat_t out)const
00254     {CheckAxis();v_copy(axis,out);out[3]=alpha>epsilon?-norm/alpha:0;};
00255    
00260     void QuaternionAngleDerivative(CQuat_t out)const
00261     {float f=0.5*alpha;QuaternionNormDerivative(out);v4_scale(out,f,out);};
00266     void InverseQuaternionAngleDerivative(CQuat_t out)const
00267     {float f=0.5*alpha;InverseQuaternionNormDerivative(out);v4_scale(out,f,out);}; 
00272     void QuaternionAxisDerivative(CMatrix4_t out){
00273         CheckAxis();m_identity(out);out[15]=0;m_rescale(norm,out,out);//can be optimized
00274     }
00275     void QuaternionAxisAngleDerivative(CMatrix4_t out)
00276     {CheckAxis(); float f=0.5*alpha;m_identity(out);out[15]=0;m_rescale(f,out,out);};//can be optimized
00277 
00278     void InverseQuaternionNormDerivative(CQuat_t out)const
00279     {CheckAxis();v_scale(axis,-1,out);out[3]=alpha>epsilon?-norm/alpha:0;};
00280 
00285     int SetAngle(float angle);
00286     void SetNorm(float no){v_scale(axis,no,beta);alpha=sqrt(1-no*no);norm=no;}
00287     int SetRotationAxis(const CVector3_t a);
00288     int SetRotationParam(CVector3_t param);
00289     float GetAngle()const {return 2*asin(norm);}
00290     float GetAlpha()const {return alpha;}
00291     void Invert(){v_scale(beta,-1,beta);v_scale(axis,-1,axis);}
00292     void RandomAxis();
00293     //  void RandomAngle(){SetAngle(RND(2*pi)-pi);}
00294     void RandomAngle(){SetAngle(RND(pi)-pi/2);}
00295     void Identity(){v_clear(beta);alpha=1;}
00296     void MinimizeRotation(Rotation& rot);
00297     void MinimizeRotation(const CQuat_t q);
00298     float MinimizePositionAndRotationAngle(const CVector3_t tar, const CVector3_t with, const CQuat_t rot,float k=0.5);
00299     void MinimizePositionAndRotation(const CVector3_t tar, const CVector3_t with, const CQuat_t rot,float k=0.5)
00300     {SetAngle(MinimizePositionAndRotationAngle(tar,with,rot,k));};
00301 
00305     Rotation& operator*(const Rotation& r);
00306 
00307 };
00308 
00312 ostream& operator<<(ostream& out, const Rotation& rt);
00313 
00314 
00315 istream& operator>>(istream& in, Rotation& rt);
00316 #endif
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:25 2010 for Body_Schema_Learning by  doxygen 1.6.1