Affine3DTransform.cpp

Go to the documentation of this file.

/* ImLib3D
 * Copyright (c) 2001, ULP-IPB Strasbourg.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
#include<ImLib3D/Affine3DTransform.hpp>
#include<ImLib3D/ImLib3DFile.hpp>
#include <math.h>
#include<ImLib3D/Vect3D.hpp>



Affine3DTransform::Affine3DTransform(const string &fname)
{
    Reset();
    ReadFromFile(fname);
}

void 
Affine3DTransform::Read( ImLib3DFile *file,xmlpp::Element *parentNode,xmlpp::Element *node)
{
    node=CreateReadNode("Affine3DTransform",file,parentNode,node);
    xmlpp::Element *mNode=file->GetChild(node,"M");
    matrix(0,0)=file->GetDoubleAttribute(mNode,"a11");
    matrix(1,0)=file->GetDoubleAttribute(mNode,"a21");
    matrix(2,0)=file->GetDoubleAttribute(mNode,"a31");
    matrix(0,1)=file->GetDoubleAttribute(mNode,"a12");
    matrix(1,1)=file->GetDoubleAttribute(mNode,"a22");
    matrix(2,1)=file->GetDoubleAttribute(mNode,"a32");
    matrix(0,2)=file->GetDoubleAttribute(mNode,"a13");
    matrix(1,2)=file->GetDoubleAttribute(mNode,"a23");
    matrix(2,2)=file->GetDoubleAttribute(mNode,"a33");
    xmlpp::Element *tNode=file->GetChild(node,"T");
    translation(0)=file->GetDoubleAttribute(tNode,"t1");
    translation(1)=file->GetDoubleAttribute(tNode,"t2");
    translation(2)=file->GetDoubleAttribute(tNode,"t3");
}


void 
Affine3DTransform::Write(ImLib3DFile *file,xmlpp::Element *parentNode,xmlpp::Element *node) const
{
    node=CreateWriteNode("Affine3DTransform",file,parentNode,node);
    xmlpp::Element *mNode=node->add_child("M");

    mNode->set_attribute("a11",SPrintf("%10.10f",matrix(0,0)));
    mNode->set_attribute("a21",SPrintf("%10.10f",matrix(1,0)));
    mNode->set_attribute("a31",SPrintf("%10.10f",matrix(2,0)));
    mNode->set_attribute("a12",SPrintf("%10.10f",matrix(0,1)));
    mNode->set_attribute("a22",SPrintf("%10.10f",matrix(1,1)));
    mNode->set_attribute("a32",SPrintf("%10.10f",matrix(2,1)));
    mNode->set_attribute("a13",SPrintf("%10.10f",matrix(0,2)));
    mNode->set_attribute("a23",SPrintf("%10.10f",matrix(1,2)));
    mNode->set_attribute("a33",SPrintf("%10.10f",matrix(2,2)));
    xmlpp::Element *tNode=node->add_child("T");
    tNode->set_attribute("t1",SPrintf("%10.10f",translation(0)));
    tNode->set_attribute("t2",SPrintf("%10.10f",translation(1)));
    tNode->set_attribute("t3",SPrintf("%10.10f",translation(2)));
}

void Affine3DTransform::Inverse()
{
    matrix.Inverse();
    translation=-(matrix*translation);
}
Affine3DTransform operator*(const Affine3DTransform& a1,const Affine3DTransform& a2)
{
    Affine3DTransform res;
    res.translation=a1.matrix*a2.translation+a1.translation;
    res.matrix=a1.matrix*a2.matrix;
    return res;
}

Affine3DTransform Affine3DTransform::operator*=(const Affine3DTransform& other)
{
    translation=matrix*other.translation+translation;
    matrix*=other.matrix;
    return *this;
}
void Affine3DTransform::SetRotation(float theta,Vect3Df direction,
                                    const Vect3Df &center)
{
    if(direction.Norm2()==0){SetRotationX(0);}
//      float c1 = cos(theta);
//      float s1 = sin(theta);
    direction.Unit();
    Vect3Df U=direction;
    float initS[]={  0.0  , -direction.z ,  direction.y ,
                     direction.z  ,  0.0 , -direction.x ,
                     -direction.y ,  direction.x ,  0.0  };
    Matrix S(initS);

    Matrix I; 
    I.SetToIdentity();

    Matrix UUt;
    for(int i=0;i<3;i++)
    for(int j=0;j<3;j++)
    {
        UUt(i,j)=U(i)*U(j);
    }

    matrix=UUt+cos(theta)*(I-UUt)+sin(theta)*S;

    translation=center-matrix*center;
}
void Affine3DTransform::SetRotationX(float theta,const Vect3Df &center)
{
    float c1 = cos(theta);
    float s1 = sin(theta);

    float initmatrix[]={
        1 ,  0  ,   0,
        0 ,  c1 , -s1,
        0 ,  s1 ,  c1 };
    matrix=Matrix(initmatrix);

    translation=center-matrix*center;
}

void Affine3DTransform::SetRotationY(float theta,const Vect3Df &center)
{
    float c1 = cos(theta);
    float s1 = sin(theta);
    float initMatrix[]={
        c1    , 0   , s1,
        0     , 1   , 0,
        (-s1) , 0   , c1};
    matrix=Matrix(initMatrix);
    translation=center-matrix*center;
}

void Affine3DTransform::SetRotationZ(float theta,const Vect3Df &center)
{
    float c1 = cos(theta);
    float s1 = sin(theta);

    float initMatrix[]={
        c1  ,  (-s1)   , 0,
        s1  ,   c1     , 0,
        0   ,   0      , 1};

    matrix=Matrix(initMatrix);
    translation=center-matrix*center;
}


//  void Affine3DTransform::SetRotation(float rad, float x, float y, float z)
//  {
//      Vect3Df V(x, y, z);
//      V.Unit();
    
//      float s=sin(rad/2.0);
//      V*=s;
    
//      quat qRot(cos(rad/2.0), V.x, V.y, V.z);
    
    
//  }

---