ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/utility/math/matrix/transformationmatrix3.cpp
Go to the documentation of this file.
00001 
00009 #include "transformationmatrix3.h"
00010 #include "rotationmatrix3.h"
00011 #include <argos3/core/utility/math/vector3.h>
00012 
00013 namespace argos {
00014 
00015    /****************************************/
00016    /****************************************/   
00017 
00018    void CTransformationMatrix3::SetFromComponents(const CRotationMatrix3& c_rotation, const CVector3& c_translation) {
00019       /* Set the rotation elements */
00020       SetRotationMatrix(c_rotation);
00021       
00022       /* Set the translation elements */
00023       SetTranslationVector(c_translation);
00024       
00025       /* Set the bottom row elements */
00026       m_pfValues[12] = 0.0f; m_pfValues[13] = 0.0f; m_pfValues[14] = 0.0f; m_pfValues[15] = 1.0f;
00027    }
00028 
00029    /****************************************/
00030    /****************************************/   
00031 
00032    void CTransformationMatrix3::SetFromMatrix(const CMatrix<4,4>& c_matrix) {
00033       m_pfValues[ 0] = c_matrix.m_pfValues[ 0];
00034       m_pfValues[ 1] = c_matrix.m_pfValues[ 1];
00035       m_pfValues[ 2] = c_matrix.m_pfValues[ 2];
00036       m_pfValues[ 3] = c_matrix.m_pfValues[ 3];
00037       m_pfValues[ 4] = c_matrix.m_pfValues[ 4];
00038       m_pfValues[ 5] = c_matrix.m_pfValues[ 5];
00039       m_pfValues[ 6] = c_matrix.m_pfValues[ 6];
00040       m_pfValues[ 7] = c_matrix.m_pfValues[ 7];
00041       m_pfValues[ 8] = c_matrix.m_pfValues[ 8];
00042       m_pfValues[ 9] = c_matrix.m_pfValues[ 9];
00043       m_pfValues[10] = c_matrix.m_pfValues[10];
00044       m_pfValues[11] = c_matrix.m_pfValues[11];
00045       m_pfValues[12] = c_matrix.m_pfValues[12];
00046       m_pfValues[13] = c_matrix.m_pfValues[13];
00047       m_pfValues[14] = c_matrix.m_pfValues[14];
00048       m_pfValues[15] = c_matrix.m_pfValues[15];
00049    }
00050    
00051    /****************************************/
00052    /****************************************/   
00053 
00054    void CTransformationMatrix3::SetFromValues(Real f_value0,  Real f_value1,  Real f_value2,  Real f_value3,
00055                                               Real f_value4,  Real f_value5,  Real f_value6,  Real f_value7, 
00056                                               Real f_value8,  Real f_value9,  Real f_value10, Real f_value11,
00057                                               Real f_value12, Real f_value13, Real f_value14, Real f_value15) {
00058       m_pfValues[ 0] = f_value0;
00059       m_pfValues[ 1] = f_value1;
00060       m_pfValues[ 2] = f_value2;
00061       m_pfValues[ 3] = f_value3;
00062       m_pfValues[ 4] = f_value4;
00063       m_pfValues[ 5] = f_value5;
00064       m_pfValues[ 6] = f_value6;
00065       m_pfValues[ 7] = f_value7;
00066       m_pfValues[ 8] = f_value8;
00067       m_pfValues[ 9] = f_value9;
00068       m_pfValues[10] = f_value10;
00069       m_pfValues[11] = f_value11;
00070       m_pfValues[12] = f_value12;
00071       m_pfValues[13] = f_value13;
00072       m_pfValues[14] = f_value14;
00073       m_pfValues[15] = f_value15;
00074    }
00075    
00076    /****************************************/
00077    /****************************************/   
00078 
00079    void CTransformationMatrix3::SetRotationMatrix(const CRotationMatrix3& c_rotation) {
00080       m_pfValues[0] = c_rotation.m_pfValues[0]; m_pfValues[1] = c_rotation.m_pfValues[1]; m_pfValues[ 2] = c_rotation.m_pfValues[2];
00081       m_pfValues[4] = c_rotation.m_pfValues[3]; m_pfValues[5] = c_rotation.m_pfValues[4]; m_pfValues[ 6] = c_rotation.m_pfValues[5];
00082       m_pfValues[8] = c_rotation.m_pfValues[6]; m_pfValues[9] = c_rotation.m_pfValues[7]; m_pfValues[10] = c_rotation.m_pfValues[8];
00083    }
00084    
00085    /****************************************/
00086    /****************************************/   
00087 
00088    const CRotationMatrix3 CTransformationMatrix3::GetRotationMatrix() const {
00089       return CRotationMatrix3(m_pfValues[0], m_pfValues[1], m_pfValues[2],
00090                               m_pfValues[4], m_pfValues[5], m_pfValues[6],
00091                               m_pfValues[8], m_pfValues[9], m_pfValues[10]);
00092    }
00093    
00094    /****************************************/
00095    /****************************************/   
00096 
00097    void CTransformationMatrix3::SetTranslationVector(const CVector3& c_translation) {
00098       m_pfValues[ 3] = c_translation.m_fX;
00099       m_pfValues[ 7] = c_translation.m_fY;
00100       m_pfValues[11] = c_translation.m_fZ; 
00101    }
00102    
00103    /****************************************/
00104    /****************************************/   
00105 
00106    const CVector3 CTransformationMatrix3::GetTranslationVector() const {
00107       return CVector3(m_pfValues[3], m_pfValues[7], m_pfValues[11]);
00108    }
00109 
00110    /****************************************/
00111    /****************************************/   
00112 
00113    CVector3 CTransformationMatrix3::operator*(const CVector3& c_vector) const {
00114       return CVector3(m_pfValues[0]*c_vector.m_fX + m_pfValues[1]*c_vector.m_fY + m_pfValues[2]*c_vector.m_fZ + c_vector.m_fX,
00115                            m_pfValues[3]*c_vector.m_fX + m_pfValues[4]*c_vector.m_fY + m_pfValues[5]*c_vector.m_fZ + c_vector.m_fY,
00116                            m_pfValues[6]*c_vector.m_fX + m_pfValues[7]*c_vector.m_fY + m_pfValues[8]*c_vector.m_fZ + c_vector.m_fZ);
00117    }
00118 
00119    /****************************************/
00120    /****************************************/
00121 }