Geometry.cpp

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 #include "Geometry.h"
00020 
00021 
00022 CRef::CRef(){
00023   Identity();
00024 }
00025 CRef::~CRef(){}
00026 
00027 void CRef::Update(){
00028   CMatrix3_t tmp;
00029   m_translate(m_origin,tmp);
00030 #ifndef KINEMATIC_TREE_VIEW
00031   m_multiply(tmp,m_orient,m_result);
00032 #else
00033   m_multiply(m_orient,tmp,m_result);
00034 #endif
00035   if(m_absRef!=NULL){
00036         m_absRef->Update();
00037     m_multiply(*m_absRef->GetAbsRef(),m_result,m_absResult);
00038     m_multiply(*m_absRef->GetAbsRef(),tmp,m_absResultPos);
00039   }else{
00040     m_copy(m_result,m_absResult);
00041     m_copy(tmp,m_absResultPos);
00042   }
00043 }
00044 
00045 void CRef::Identity(){
00046   m_absRef  = NULL;
00047   v_set(0,0,0,m_origin);  
00048   m_identity(m_orient);
00049   m_identity(m_result);
00050   m_identity(m_absResult);
00051 }
00052 
00053 CMatrix4_t *CRef::GetRef(){
00054   return &m_result;
00055 }
00056 
00057 CMatrix4_t *CRef::GetAbsRef(){
00058   return &m_absResult;
00059 }
00060 
00061 CMatrix4_t *CRef::GetAbsRefPos(){
00062   return &m_absResultPos;
00063 }
00064 
00065 void CRef::Copy(CRef_t *src){
00066   m_absRef = src->m_absRef;
00067   v_copy(src->m_origin,m_origin);
00068   m_copy(src->m_orient,m_orient);
00069   Update();
00070 }
00071 
 All Data Structures Functions Variables

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