Frame.cpp

Go to the documentation of this file.

/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 3      *
*                (c) 2006-2008 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library 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 Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                              SOFA :: Framework                              *
*                                                                             *
* Authors: M. Adam, J. Allard, B. Andre, P-J. Bensoussan, S. Cotin, C. Duriez,*
* H. Delingette, F. Falipou, F. Faure, S. Fonteneau, L. Heigeas, C. Mendoza,  *
* M. Nesme, P. Neumann, J-P. de la Plata Alcade, F. Poyer and F. Roy          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
/* Francois Faure, INRIA-UJF, 2006
 */
#include <sofa/defaulttype/Frame.h>
#include <iostream>
using std::cout;
using std::endl;

namespace sofa
{

namespace defaulttype
{

Frame::Frame (const Vec3 &origin, const Mat33& matrix )
                : origin_(origin)
                , basis_(matrix)
{}
Frame::Frame (const Vec3 &origin, const Quat &orientation, const Vec3& scale )
{
        setTransform( origin, orientation, scale );
}

Frame::Frame (const Vec3 &origin )
                : origin_(origin)
{
        basis_.identity();
}

Frame::Frame ()
{
        clear();
}

void Frame::clear()
{
        basis_.identity();
        origin_.clear();
}

//void clear() { origin_.clear(); basis__.clear(); scale_=Vec3(1,1,1); }

Frame::Vec3& Frame::getOrigin ()
{
        return origin_;
}
const Frame::Vec3& Frame::getOrigin () const
{
        return origin_;
}
void Frame::setOrigin( const Frame::Vec3& origin )
{
        origin_=origin;
}

Frame::Mat33& Frame::getBasis ()
{
        return basis_;
}
const Frame::Mat33& Frame::getBasis () const
{
        return basis_;
}
void Frame::setBasis( const Frame::Mat33& m )
{
        basis_=m;
}

void Frame::setTransform( const Frame::Vec3& origin, const Frame::Quat& orientation, const Frame::Vec3& scale )
{
        setOrigin(origin);
        orientation.toMatrix(basis_);
        for( int i=0; i<3; i++ ) {
                basis_[i][0] *= scale[0];
                basis_[i][1] *= scale[1];
                basis_[i][2] *= scale[2];
        }
}



Frame Frame::identity()
{
        Frame c;
        c.clear();
        return c;
}

void Frame::multRight( const Frame& c )
{
        origin_ += basis_ * c.getOrigin();
        basis_ = basis_ * c.getBasis();
}

Frame Frame::mult( const Frame& c ) const
{
        Frame r = (*this);
        r.multRight(c);
        return r;
}

void Frame::writeOpenGlMatrix( float *m ) const
{
        m[0] = (float) basis_[0][0];
        m[1] = (float) basis_[1][0];
        m[2] = (float) basis_[2][0];
        m[3] = (float) 0;
        m[4] = (float) basis_[0][1];
        m[5] = (float) basis_[1][1];
        m[6] = (float) basis_[2][1];
        m[7] = (float) 0;
        m[8] = (float) basis_[0][2];
        m[9] = (float) basis_[1][2];
        m[10] = (float) basis_[2][2];
        m[11] = (float) 0;
        m[12] = (float) origin_[0];
        m[13] = (float) origin_[1];
        m[14] = (float) origin_[2];
        m[15] = (float) 1;
}

void Frame::writeOpenGlMatrix( double *m ) const
{
        m[0] = basis_[0][0];
        m[1] = basis_[1][0];
        m[2] = basis_[2][0];
        m[3] = 0;
        m[4] = basis_[0][1];
        m[5] = basis_[1][1];
        m[6] = basis_[2][1];
        m[7] = 0;
        m[8] = basis_[0][2];
        m[9] = basis_[1][2];
        m[10] = basis_[2][2];
        m[11] = 0;
        m[12] = origin_[0];
        m[13] = origin_[1];
        m[14] = origin_[2];
        m[15] = 1;
}

Frame Frame::inversed() const
{
        Mat33 inv;
#ifdef NDEBUG
        inv.invert( basis_ );
#else
        bool invertible = inv.invert( basis_ );
        assert( invertible );
#endif
        return Frame( -(inv*origin_) , inv );
}

std::ostream& operator << (std::ostream& out, const sofa::defaulttype::Frame& c )
{
    out<<"origin = "<<c.getOrigin()<<", basis matrix="<<endl;
    for( int i=0; i<3; i++ ) {
        for( int j=0; j<3; j++ )
            out<<c.getBasis()[i][j]<<" ";
        out<<endl;
    }
    return out;
}

} // namespace defaulttype

} // namespace sofa