$search

rpp_vecmat.cpp

Go to the documentation of this file.

/* ========================================================================
 * PROJECT: ARToolKitPlus
 * ========================================================================
 * This work is based on the original ARToolKit developed by
 *   Hirokazu Kato
 *   Mark Billinghurst
 *   HITLab, University of Washington, Seattle
 * http://www.hitl.washington.edu/artoolkit/
 *
 * Copyright of the derived and new portions of this work
 *     (C) 2006 Graz University of Technology
 *
 * This framework 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 framework 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 framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * For further information please contact 
 *   Dieter Schmalstieg
 *   <schmalstieg@icg.tu-graz.ac.at>
 *   Graz University of Technology, 
 *   Institut for Computer Graphics and Vision,
 *   Inffeldgasse 16a, 8010 Graz, Austria.
 * ========================================================================
 ** @author   Thomas Pintaric
 *
 * $Id: rpp_vecmat.cpp 162 2006-04-19 21:28:10Z grabner $
 * @file
 * ======================================================================== */


#ifndef _NO_LIBRPP_


#include "rpp/rpp_vecmat.h"
#include "math.h"
#include "assert.h"
#include <stdlib.h>
#include <stdio.h>


namespace rpp {


int svdcmp( double **a, int m,int n, double *w, double **v);
int quintic(double [], double [], double [], int*, double);
int quartic(double[], double[], double[], int* );
int cubic(double[], double[], int*);



#ifdef _WIN32_WCE
        real_t _sin(real_t a) { return(real_t(::sin(a))); }
        real_t _cos(real_t a) { return(real_t(::cos(a))); }
        real_t _atan2(real_t a, real_t b) { return(real_t(::atan2(a,b))); }
        real_t _abs(real_t a) { return((a>0?a:-a)); }
        real_t _acos(real_t a) { return(real_t(::acos(a))); }
        real_t _sqrt(real_t a) { return(real_t(::sqrt(a))); }
        real_t _pow(real_t a, real_t b) { return(real_t(::pow(a,b))); }
#else
        real_t _sin(real_t a) { return(::sin(a)); }
        real_t _cos(real_t a) { return(::cos(a)); }
        real_t _atan2(real_t a, real_t b) { return(::atan2(a,b)); }
        real_t _abs(real_t a) { return((a>0?a:-a)); }
        real_t _acos(real_t a) { return(::acos(a)); }
        real_t _sqrt(real_t a) { return(::sqrt(a)); }
        real_t _pow(real_t a, real_t b) { return(::pow(a,b)); }
        /*
        real_t _sin(real_t a) { return(::sinf(a)); }
        real_t _cos(real_t a) { return(::cosf(a)); }
        real_t _atan2(real_t a, real_t b) { return(::atan2f(a,b)); }
        real_t _abs(real_t a) { return((a>0?a:-a)); }
        real_t _acos(real_t a) { return(::acosf(a)); }
        real_t _sqrt(real_t a) { return(::sqrtf(a)); }
        real_t _pow(real_t a, real_t b) { return(::powf(a,b)); }
        */
#endif

// ---------------------------------------------------------------------------

void mat33_from_double_pptr(mat33_t &mat, double** m_ptr)
{
        for(int m=0; m<3; m++)
                for(int n=0; n<3; n++)
                        mat.m[m][n] = (real_t) m_ptr[m][n];
}


void mat33_from_float_pptr(mat33_t &mat, float** m_ptr)
{
        for(int m=0; m<3; m++)
                for(int n=0; n<3; n++)
                        mat.m[m][n] = (real_t) m_ptr[m][n];
}


double ** mat33_to_double_pptr(const mat33_t &mat)
{
        double **M = (double**) malloc(3*sizeof(double*));
        for(int i=0; i<3; i++) M[i] = (double*) malloc(3*sizeof(double));
        for(int m=0; m<3; m++)
                for(int n=0; n<3; n++)
                        M[m][n] = (double) mat.m[m][n];
        return(M);
}


float ** mat33_to_float_pptr(const mat33_t &mat)
{
        float **M = (float**) malloc(3*sizeof(float*));
        for(int i=0; i<3; i++) M[i] = (float*) malloc(3*sizeof(float));
        for(int m=0; m<3; m++)
                for(int n=0; n<3; n++)
                        M[m][n] = (float) mat.m[m][n];
        return(M);
}


void free_double_pptr(double*** m_ptr)
{
        for(int i=0; i<3; i++) free((*m_ptr)[i]);
        free(*m_ptr);
}

void free_float_pptr(float*** m_ptr)
{
        for(int i=0; i<3; i++) free((*m_ptr)[i]);
        free(*m_ptr);
}

void vec3_from_double_ptr(vec3_t &vec, double* v_ptr)
{
        vec.v[0] = (real_t)v_ptr[0];
        vec.v[1] = (real_t)v_ptr[1];
        vec.v[2] = (real_t)v_ptr[2];
}

void vec3_from_float_ptr(vec3_t &vec, float* v_ptr)
{
        vec.v[0] = (real_t)v_ptr[0];
        vec.v[1] = (real_t)v_ptr[1];
        vec.v[2] = (real_t)v_ptr[2];
}

double* vec3_to_double_ptr(const vec3_t &vec)
{
        double* v = (double*) malloc(3* sizeof(double));
        v[0] = (double) vec.v[0];
        v[1] = (double) vec.v[1];
        v[2] = (double) vec.v[2];
        return(v);
}

float* vec3_to_float_ptr(const vec3_t &vec)
{
        float* v = (float*) malloc(3* sizeof(float));
        v[0] = (float) vec.v[0];
        v[1] = (float) vec.v[1];
        v[2] = (float) vec.v[2];
        return(v);
}


void free_double_ptr(double** v_ptr)
{
        free(*v_ptr);
}

void free_float_ptr(float** v_ptr)
{
        free(*v_ptr);
}



void mat33_assign(mat33_t &m,
                                  const real_t m00, const real_t m01, const real_t m02,
                                  const real_t m10, const real_t m11, const real_t m12,
                                  const real_t m20, const real_t m21, const real_t m22)
{
        m.m[0][0] = m00;        m.m[0][1] = m01;        m.m[0][2] = m02;
        m.m[1][0] = m10;        m.m[1][1] = m11;        m.m[1][2] = m12;
        m.m[2][0] = m20;        m.m[2][1] = m21;        m.m[2][2] = m22;
}


void _dbg_quat_print(const quat_t &q, char* name)
{
        printf("%s:  s = [ %.4f ]  v = [ ",name,q.s);
        for(unsigned int c=0; c<3; c++)
        {
                // double d = (double)(q.v.v[c]);
                printf("%.4f ",q.v.v[c]);
        }
        printf("]\n");
}

void _dbg_mat33_print(const mat33_t &m, char* name)
{
        printf("%s:\n",name);
        for(unsigned int r=0; r<3; r++)
        {
                printf("[ ");
                for(unsigned int c=0; c<3; c++)
                {
                        double d = (double)(m.m[r][c]);
                        printf("%.4f ",d);
                }
                printf("]\n");
        }
}

void _dbg_mat33_array_print(const mat33_array &m, char* name)
{
        for(unsigned int i=0; i<m.size(); i++)
        {
                printf("%s.at(%i):\n",name,i);
                for(unsigned int r=0; r<3; r++)
                {
                        printf("[ ");
                        for(unsigned int c=0; c<3; c++)
                        {
                                double d = (double)(m.at(i).m[r][c]);
                                printf("%.4f ",d);
                        }
                        printf("]\n");
                }
        }
}

void _dbg_vec3_print(const vec3_t &v, char* name)
{
        printf("%s:  [ ",name);
        for(unsigned int c=0; c<3; c++)
        {
                double d = (double)(v.v[c]);
                printf("%.4f ",d);
        }
        printf("]\n");
}

void _dbg_vec3_array_print(const vec3_array &v, char* name)
{
        for(unsigned int i=0; i<v.size(); i++)
        {
                printf("%s.at(%i):  [ ",name,i);
                for(unsigned int c=0; c<3; c++)
                {
                        double d = (double)(v.at(i).v[c]);
                        printf("%.4f ",d);
                }
                printf("]\n");
        }
}


bool _dbg_load_vec3_array(vec3_array &va, char* filename)
{
        FILE *fp = fopen( filename, "r" );
        if( fp == NULL ) return(false);

        int n;
        int lineno = 0;
        va.clear();

        while(!feof(fp))
        {
                ++lineno;
                double vx,vy,vz;
                vec3_t v;
                n = fscanf(fp, "%lf%lf%lf\n", &vx,&vy,&vz);
                if((n!=3) || ferror(fp))
                {
                        printf("file i/o error: %s (line %i)",filename,lineno);
                        fclose(fp);
                        return(lineno > 1);
                }
                v.v[0] = (real_t)vx;
                v.v[1] = (real_t)vy;
                v.v[2] = (real_t)vz;
                va.push_back(v);
        }

        fclose(fp);
        return(true);
}


void vec3_assign(vec3_t &v, const real_t x, const real_t y, const real_t z)
{
        v.v[0] = x;
        v.v[1] = y;
        v.v[2] = z;
}

void vec3_clear(vec3_t &v)
{
        v.v[0] = 0;
        v.v[1] = 0;
        v.v[2] = 0;
}

void vec3_copy(vec3_t &a, const vec3_t &b)
{
        a.v[0] = b.v[0];
        a.v[1] = b.v[1];
        a.v[2] = b.v[2];
}


void vec3_array_sum(vec3_t &v_sum2, const vec3_array &va)
{
        vec3_clear(v_sum2);
        for(vec3_array_const_iter iter = va.begin();
                iter != va.end(); iter++)
        {
                v_sum2.v[0] += (*iter).v[0];
                v_sum2.v[1] += (*iter).v[1];
                v_sum2.v[2] += (*iter).v[2];
        }
}

void vec3_array_sum(scalar_array &v_sum1, const vec3_array &va)
{
        v_sum1.clear();
        v_sum1.resize(va.size());

        for(unsigned int i=0; i<va.size(); i++)
                v_sum1.at(i) = (va[i].v[0] + va[i].v[1] + va[i].v[2]);
}

void vec3_array_pow2(vec3_array &va)
{
        for(vec3_array_iter iter = va.begin();
                iter != va.end(); iter++)
        {
                (*iter).v[0] *= (*iter).v[0];
                (*iter).v[1] *= (*iter).v[1];
                (*iter).v[2] *= (*iter).v[2];
        }
}




void vec3_div(vec3_t &va, const real_t n)
{
        va.v[0] /= n;
        va.v[1] /= n;
        va.v[2] /= n;
}

void vec3_div(vec3_t &va, const vec3_t &vb)
{
        va.v[0] /= vb.v[0];
        va.v[1] /= vb.v[1];
        va.v[2] /= vb.v[2];
}

void vec3_mult(vec3_t &va, const real_t n)
{
        va.v[0] *= n;
        va.v[1] *= n;
        va.v[2] *= n;
}

void vec3_mult(vec3_t &va, const vec3_t &vb)
{
        va.v[0] *= vb.v[0];
        va.v[1] *= vb.v[1];
        va.v[2] *= vb.v[2];
}

void vec3_add(vec3_t &va, const real_t f)
{
        va.v[0] += f;
        va.v[1] += f;
        va.v[2] += f;
}

void vec3_add(vec3_t &va, const vec3_t &vb)
{
        va.v[0] += vb.v[0];
        va.v[1] += vb.v[1];
        va.v[2] += vb.v[2];
}

void vec3_add(vec3_t &va, const vec3_t &vb, const vec3_t &vc)
{
        va.v[0] = vb.v[0] + vc.v[0];
        va.v[1] = vb.v[1] + vc.v[1];
        va.v[2] = vb.v[2] + vc.v[2];
}

void vec3_sub(vec3_t &va, const real_t f)
{
        va.v[0] -= f;
        va.v[1] -= f;
        va.v[2] -= f;
}

void vec3_sub(vec3_t &va, const vec3_t &vb)
{
        va.v[0] -= vb.v[0];
        va.v[1] -= vb.v[1];
        va.v[2] -= vb.v[2];
}

void vec3_sub(vec3_t &va, const vec3_t &vb, const vec3_t &vc)
{
        va.v[0] = vb.v[0] - vc.v[0];
        va.v[1] = vb.v[1] - vc.v[1];
        va.v[2] = vb.v[2] - vc.v[2];
}

real_t vec3_dot(const vec3_t &va, const vec3_t &vb)
{
        return(va.v[0]*vb.v[0]+va.v[1]*vb.v[1]+va.v[2]*vb.v[2]);
}

void vec3_cross(vec3_t &va, const vec3_t &vb, const vec3_t &vc)
{
        va.v[0] = (vb.v[1] * vc.v[2] - vc.v[1] * vb.v[2]);
        va.v[1] = (vb.v[2] * vc.v[0] - vc.v[2] * vb.v[0]);
        va.v[2] = (vb.v[0] * vc.v[1] - vc.v[0] * vb.v[1]);
}

real_t vec3_norm(const vec3_t &v)
{
        return(_sqrt(v.v[0]*v.v[0] + v.v[1]*v.v[1] + v.v[2]*v.v[2]));
}

real_t vec3_sum(const vec3_t &v)
{
        return(v.v[0] + v.v[1] + v.v[2]);
}

void vec3_array_add(vec3_array &va, const vec3_t &a)
{
        for(vec3_array_iter iter = va.begin();
                iter != va.end(); iter++)
        {
                (*iter).v[0] += a.v[0];
                (*iter).v[1] += a.v[1];
                (*iter).v[2] += a.v[2];
        }
}

void vec3_array_sub(vec3_array &va, const vec3_t &a) 
{
        for(vec3_array_iter iter = va.begin();
                iter != va.end(); iter++)
        {
                (*iter).v[0] -= a.v[0];
                (*iter).v[1] -= a.v[1];
                (*iter).v[2] -= a.v[2];
        }
}

void vec3_array_set(vec3_array &va, const vec3_t &a, const bool mask[3])
{
        for(vec3_array_iter iter = va.begin();
                iter != va.end(); iter++)
        {
                if(mask[0]) (*iter).v[0] = a.v[0];
                if(mask[1]) (*iter).v[1] = a.v[1];
                if(mask[2]) (*iter).v[2] = a.v[2];
        }
}

void vec3_array_mult(vec3_array &va, const scalar_array &c)
{
        assert(va.size() == c.size());
        for(unsigned int i=0; i<va.size(); i++)
        {
                va[i].v[0] *= c[i];
                va[i].v[1] *= c[i];
                va[i].v[2] *= c[i];
        }
}

void vec3_array_mean(vec3_t &v_mean, const vec3_array &va)
{
        vec3_array_sum(v_mean, va);
        real_t l = (real_t) va.size();
        vec3_div(v_mean, l);
}


void vec3_mul_vec3trans(mat33_t &m, const vec3_t &va, const vec3_t &vb)
{
        m.m[0][0] = va.v[0] * vb.v[0];
        m.m[0][1] = va.v[0] * vb.v[1];
        m.m[0][2] = va.v[0] * vb.v[2];
        m.m[1][0] = va.v[1] * vb.v[0];
        m.m[1][1] = va.v[1] * vb.v[1];
        m.m[1][2] = va.v[1] * vb.v[2];
        m.m[2][0] = va.v[2] * vb.v[0];
        m.m[2][1] = va.v[2] * vb.v[1];
        m.m[2][2] = va.v[2] * vb.v[2];
}

real_t vec3trans_mul_vec3(const vec3_t &va, const vec3_t &vb)
{
        return(va.v[0] * vb.v[0] + va.v[1] * vb.v[1] + va.v[2] * vb.v[2]);
}


void mat33_clear(mat33_t &m)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        m.m[r][c] = 0;
                }
}

void mat33_copy(mat33_t &md, const mat33_t &ms)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        md.m[r][c] = ms.m[r][c];
                }
}


void mat33_to_col_vec3(vec3_t &c0, vec3_t &c1, vec3_t &c2, const mat33_t &m)
{
        for(unsigned int r=0; r<3; r++)
        {
                c0.v[r] = m.m[r][0];
                c1.v[r] = m.m[r][1];
                c2.v[r] = m.m[r][2];
        }
}


void mat33_div(mat33_t &m, const real_t f)
{
        m.m[0][0] /= f;
        m.m[0][1] /= f;
        m.m[0][2] /= f;
        m.m[1][0] /= f;
        m.m[1][1] /= f;
        m.m[1][2] /= f;
        m.m[2][0] /= f;
        m.m[2][1] /= f;
        m.m[2][2] /= f;
}

void mat33_eye(mat33_t &m)
{
        m.m[0][0] = 1;
        m.m[0][1] = 0;
        m.m[0][2] = 0;
        m.m[1][0] = 0;
        m.m[1][1] = 1;
        m.m[1][2] = 0;
        m.m[2][0] = 0;
        m.m[2][1] = 0;
        m.m[2][2] = 1;
}

real_t mat33_sum(const mat33_t &m)
{
        real_t sum(0.0f);
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        sum += m.m[r][c];
                }
                return(sum);
}


bool mat33_all_zeros(const mat33_t &m)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        if(m.m[r][c] != 0) return(false);
                }
                return(true);
}

void mat33_set_all_zeros(mat33_t &m)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        m.m[r][c] = 0;
                }
}


void mat33_array_sum(mat33_t &s, const mat33_array &ma)
{
        mat33_clear(s);
        for(mat33_array_const_iter iter = ma.begin();
                iter != ma.end(); iter++)
        {
                for(unsigned int c=0; c<3; c++)
                {
                        s.m[0][c] += (*iter).m[0][c];
                        s.m[1][c] += (*iter).m[1][c];
                        s.m[2][c] += (*iter).m[2][c];
                }
        }
}


void mat33_sub(mat33_t &mr, const mat33_t &ma, const mat33_t &mb)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        mr.m[r][c] = ma.m[r][c]-mb.m[r][c];
                }
}

void mat33_sub(mat33_t &ma, const mat33_t &mb)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        ma.m[r][c] -= mb.m[r][c];
                }
}

void mat33_add(mat33_t &mr, const mat33_t &ma, const mat33_t &mb)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        mr.m[r][c] = ma.m[r][c]+mb.m[r][c];
                }
}

void mat33_add(mat33_t &ma, const mat33_t &mb)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        ma.m[r][c] += mb.m[r][c];
                }
}


real_t mat33_det(const mat33_t &a)
{
        real_t determinant = a.m[0][0]*a.m[1][1]*a.m[2][2] + a.m[0][1]*a.m[1][2]*a.m[2][0] +
                a.m[0][2]*a.m[1][0]*a.m[2][1] - a.m[2][0]*a.m[1][1]*a.m[0][2] - 
                a.m[2][1]*a.m[1][2]*a.m[0][0] - a.m[2][2]*a.m[1][0]*a.m[0][1];
        return(determinant);
}

void mat33_inv(mat33_t &mi, const mat33_t &ma)
{
        real_t determinant = mat33_det(ma);
        mi.m[0][0] = (ma.m[1][1]*ma.m[2][2] - ma.m[1][2]*ma.m[2][1])/determinant;
        mi.m[0][1] = (ma.m[0][2]*ma.m[2][1] - ma.m[0][1]*ma.m[2][2])/determinant;
        mi.m[0][2] = (ma.m[0][1]*ma.m[1][2] - ma.m[0][2]*ma.m[1][1])/determinant;

        mi.m[1][0] = (ma.m[1][2]*ma.m[2][0] - ma.m[1][0]*ma.m[2][2])/determinant;
        mi.m[1][1] = (ma.m[0][0]*ma.m[2][2] - ma.m[0][2]*ma.m[2][0])/determinant;
        mi.m[1][2] = (ma.m[0][2]*ma.m[1][0] - ma.m[0][0]*ma.m[1][2])/determinant;

        mi.m[2][0] = (ma.m[1][0]*ma.m[2][1] - ma.m[1][1]*ma.m[2][0])/determinant;
        mi.m[2][1] = (ma.m[0][1]*ma.m[2][0] - ma.m[0][0]*ma.m[2][1])/determinant;
        mi.m[2][2] = (ma.m[0][0]*ma.m[1][1] - ma.m[0][1]*ma.m[1][0])/determinant;
}

void mat33_mult(mat33_t &m0, const mat33_t &m1, const mat33_t &m2)
{
        m0.m[0][0] = m1.m[0][0]*m2.m[0][0] + m1.m[0][1]*m2.m[1][0] + m1.m[0][2]*m2.m[2][0];
        m0.m[0][1] = m1.m[0][0]*m2.m[0][1] + m1.m[0][1]*m2.m[1][1] + m1.m[0][2]*m2.m[2][1];
        m0.m[0][2] = m1.m[0][0]*m2.m[0][2] + m1.m[0][1]*m2.m[1][2] + m1.m[0][2]*m2.m[2][2];
        m0.m[1][0] = m1.m[1][0]*m2.m[0][0] + m1.m[1][1]*m2.m[1][0] + m1.m[1][2]*m2.m[2][0];
        m0.m[1][1] = m1.m[1][0]*m2.m[0][1] + m1.m[1][1]*m2.m[1][1] + m1.m[1][2]*m2.m[2][1];
        m0.m[1][2] = m1.m[1][0]*m2.m[0][2] + m1.m[1][1]*m2.m[1][2] + m1.m[1][2]*m2.m[2][2];
        m0.m[2][0] = m1.m[2][0]*m2.m[0][0] + m1.m[2][1]*m2.m[1][0] + m1.m[2][2]*m2.m[2][0];
        m0.m[2][1] = m1.m[2][0]*m2.m[0][1] + m1.m[2][1]*m2.m[1][1] + m1.m[2][2]*m2.m[2][1];
        m0.m[2][2] = m1.m[2][0]*m2.m[0][2] + m1.m[2][1]*m2.m[1][2] + m1.m[2][2]*m2.m[2][2];
}

void mat33_mult(mat33_t &mr, const real_t n)
{

        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        mr.m[r][c] *= n;
                }
}

void mat33_transpose(mat33_t &t, const mat33_t m)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        t.m[r][c] = m.m[c][r];
                }
}

void vec3_mult(vec3_t &v0, const mat33_t &m1, const vec3_t &v2)
{
        v0.v[0] = m1.m[0][0]*v2.v[0] + m1.m[0][1]*v2.v[1] + m1.m[0][2]*v2.v[2];
        v0.v[1] = m1.m[1][0]*v2.v[0] + m1.m[1][1]*v2.v[1] + m1.m[1][2]*v2.v[2];
        v0.v[2] = m1.m[2][0]*v2.v[0] + m1.m[2][1]*v2.v[1] + m1.m[2][2]*v2.v[2];
}

void vec3_array_mult(vec3_array &va, const mat33_t &m, const vec3_array &vb)
{
        va.clear();
        va.resize(vb.size());
        for(unsigned int i=0; i<vb.size(); i++)
        {
                vec3_mult(va.at(i),m,vb.at(i));
        }
}

void mat33_svd2(mat33_t &u, mat33_t &s, mat33_t &v, const mat33_t &m)
{
        mat33_clear(u);
        mat33_clear(v);

        double** m_ptr = mat33_to_double_pptr(m);
        double** v_ptr = mat33_to_double_pptr(v);

        vec3_t q;
        vec3_clear(q);
        double*  q_ptr = vec3_to_double_ptr(q);

        /*int ret =*/ svdcmp(m_ptr, 3, 3, q_ptr, v_ptr);

        mat33_from_double_pptr(u,m_ptr);
        mat33_from_double_pptr(v,v_ptr);
        vec3_from_double_ptr(q,q_ptr);

        mat33_clear(s);
        s.m[0][0] = (real_t)q_ptr[0];
        s.m[1][1] = (real_t)q_ptr[1];
        s.m[2][2] = (real_t)q_ptr[2];

        free_double_pptr(&m_ptr);
        free_double_pptr(&v_ptr);
        free_double_ptr(&q_ptr);
}

void quat_mult(quat_t &q, const real_t s)
{
        vec3_mult(q.v,s);
        q.s *= s;
}

real_t quat_norm(const quat_t &q)
{
        const real_t f_vn = vec3_norm(q.v);
        return(_sqrt((f_vn*f_vn) + (q.s*q.s)));

}

void mat33_from_quat(mat33_t &m, const quat_t &q)
{
        const real_t a = q.s;
        const real_t b = q.v.v[0];
        const real_t c = q.v.v[1];
        const real_t d = q.v.v[2];

        m.m[0][0] = (a*a)+(b*b)-(c*c)-(d*d);
        m.m[0][1] = real_t(2.0f)*(b*c-a*d);
        m.m[0][2] = real_t(2.0f)*(b*d+a*c);

        m.m[1][0] = real_t(2.0f)*(b*c+a*d);
        m.m[1][1] = (a*a)+(c*c)-(b*b)-(d*d);
        m.m[1][2] = real_t(2.0f)*(c*d-a*b);

        m.m[2][0] = real_t(2.0f)*(b*d-a*c);
        m.m[2][1] = real_t(2.0f)*(c*d+a*b);
        m.m[2][2] = (a*a)+(d*d)-(b*b)-(c*c);
}

// ===========================================================================================
void normRv(vec3_t &n, const vec3_t &v)
{
        vec3_t _v1;
        vec3_copy(_v1,v);
        vec3_mult(_v1,_v1);
        real_t l = 1.0f / _sqrt(_v1.v[0] + _v1.v[1] + _v1.v[2]);
        vec3_copy(n,v);
        vec3_mult(n,l);
}

// ===========================================================================================
void normRv(vec3_array &normR_v, const vec3_array &v)
{
        normR_v.assign(v.begin(),v.end());
        vec3_array_pow2(normR_v);
        scalar_array _l;
        vec3_array_sum(_l,normR_v);

        for(scalar_array::iterator iter = _l.begin();
                iter != _l.end(); iter++)
        {
                (*iter) = 1.0f / _sqrt(*iter);
        }

        normR_v.assign(v.begin(),v.end());
        vec3_array_mult(normR_v,_l);
}

// ===========================================================================================
int solve_polynomial(scalar_array &r_sol, const scalar_array &coefficients)
{
        if(coefficients.size() != 5) return(0);
        r_sol.clear();

        double dd[5] = {(double)coefficients[0],
                (double)coefficients[1],
                (double)coefficients[2],
                (double)coefficients[3],
                (double)coefficients[4] };

        double sol[4] = {0,0,0,0};
        double soli[4] = {0,0,0,0};
        int n_sol = 0;
        quartic(dd, sol, soli, &n_sol);

        if(n_sol <= 0) return(0); // assert(false);

        r_sol.resize(n_sol);
        for(int i=0; i<n_sol; i++) r_sol[i] = (real_t)sol[i];
        return(n_sol);
}
// ===========================================================================================

void scalar_array_pow(scalar_array &sa, const real_t f)
{
        for(unsigned int i=0; i<sa.size(); i++) sa.at(i) = _pow(sa[i],f);
}

void scalar_array_negate(scalar_array &sa)
{
        for(unsigned int i=0; i<sa.size(); i++) sa.at(i) = - sa.at(i);
}

void scalar_array_assign(scalar_array &sa,
                                                 const  real_t f,
                                                 const unsigned int sz)
{
        sa.clear();
        sa.resize(sz);
        for(unsigned int i=0; i<sz; i++) sa.at(i) = f;
}


void scalar_array_add(scalar_array &sa, const scalar_array &sb)
{
        assert(sa.size() == sb.size());
        for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
                sa.at(i) = sa.at(i) + sb.at(i);
}

void scalar_array_clear(scalar_array &sa)
{
        for(unsigned int i=0; i<(unsigned int)sa.size(); i++) sa.at(i) = 0.;
}

void scalar_array_atan2(scalar_array &sa, 
                                                const scalar_array &sb,
                                                const scalar_array &sc)
{
        assert(sb.size() == sc.size());
        sa.clear();
        sa.resize(sb.size());
        for(unsigned int i=0; i<(unsigned int)sb.size(); i++)
                sa[i] = _atan2(sb[i],sc[i]);
}

void _dbg_scalar_array_print(const scalar_array &sa, char* name)
{
        for(unsigned int i=0; i<sa.size(); i++)
                printf("%s.at(%i):  [ %e ]\n",name,i,sa[i]);
}

void scalar_array_div(scalar_array &sa, real_t f)
{
        for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
        {
                sa.at(i) /= f;
        }
}

void scalar_array_div(scalar_array &sa, const scalar_array &sb)
{
        assert(sa.size() == sb.size());
        for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
                sa[i] /= sb[i];
}

void scalar_array_mult(scalar_array &sa, real_t f)
{
        for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
        {
                sa.at(i) *= f;
        }
}

void scalar_array_add(scalar_array &sa, real_t f)
{
        for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
        {
                sa.at(i) += f;
        }
}

void scalar_array_sub(scalar_array &sa, real_t f)
{
        for(unsigned int i=0; i<(unsigned int)sa.size(); i++)
        {
                sa.at(i) -= f;
        }
}

void mat33_pow2(mat33_t &m)
{
        for(unsigned int r=0; r<3; r++)
                for(unsigned int c=0; c<3; c++)
                {
                        m.m[r][c] *= m.m[r][c];
                }
}


void _dbg_vec3_fprint(void* fp, const vec3_t &v, char* name)
{
        fprintf((FILE*)fp,"%s:  [ ",name);
        for(unsigned int c=0; c<3; c++)
        {
                fprintf((FILE*)fp,"%.4f ",v.v[c]);
        }
        fprintf((FILE*)fp,"]\n");
}

void _dbg_mat33_fprint(void* fp, const mat33_t &m, char* name)
{
        fprintf((FILE*)fp,"%s:\n",name);
        for(unsigned int r=0; r<3; r++)
        {
                fprintf((FILE*)fp,"[ ");
                for(unsigned int c=0; c<3; c++)
                {
                        fprintf((FILE*)fp,"%.4f ",m.m[r][c]);
                }
                fprintf((FILE*)fp,"]\n");
        }
}


} // namespace rpp


#endif //_NO_LIBRPP_

---

aruco_pose
Author(s): Julian Brunner
autogenerated on Tue Dec 4 01:11:17 2012