mathlib.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 /*      3d math lib
00020  *
00021  *              written by Alexander Zaprjagaev
00022  *              frustum@public.tsu.ru
00023  */
00024 //#include <memory.h>
00025 #include "mathlib.h"
00026 
00032 float v_length(const float *v) {
00033   return (float)(sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
00034 }
00035 
00036 
00037 
00038 
00039 void v_translate(const float *m,float *out) {
00040   out[0] = m[12];
00041   out[1] = m[13];
00042   out[2] = m[14];
00043 }
00044 
00045 void v_transform(const float *v,const float *m,float *out) {
00046   float v1[3];
00047   v_copy(v,v1);
00048   out[0] = v1[0] * m[0] + v1[1] * m[4] + v1[2] * m[8] + m[12];
00049   out[1] = v1[0] * m[1] + v1[1] * m[5] + v1[2] * m[9] + m[13];
00050   out[2] = v1[0] * m[2] + v1[1] * m[6] + v1[2] * m[10] + m[14];
00051 }
00052 
00053 // multiplication when m is index 0 1 2 3//4 ...
00054 void v_transform2(const float *v,const float *m,float *out) {
00055   float v1[3];
00056   v_copy(v,v1);
00057   out[0] = v1[0] * m[0] + v1[1] * m[1] + v1[2] * m[2] + m[3];
00058   out[1] = v1[0] * m[4] + v1[1] * m[5] + v1[2] * m[6] + m[7];
00059   out[2] = v1[0] * m[8] + v1[1] * m[9] + v1[2] * m[10] + m[11];
00060 }
00061 
00062 void v_transform_normal(const float *v,const float *m,float *out) {
00063   float v1[3];
00064   v_copy(v,v1);
00065   out[0] = v1[0] * m[0] + v1[1] * m[4] + v1[2] * m[8];
00066   out[1] = v1[0] * m[1] + v1[1] * m[5] + v1[2] * m[9];
00067   out[2] = v1[0] * m[2] + v1[1] * m[6] + v1[2] * m[10];
00068 }
00069 
00070 void v_transform_normal2(const float *v,const float *m,float *out) {
00071   float v1[3];
00072   v_copy(v,v1);
00073   out[0] = v1[0] * m[0] + v1[1] * m[1] + v1[2] * m[2];
00074   out[1] = v1[0] * m[4] + v1[1] * m[5] + v1[2] * m[6];
00075   out[2] = v1[0] * m[8] + v1[1] * m[9] + v1[2] * m[10];
00076 }
00077 
00078 void m_copy(const float *m,float *out) {
00079   memcpy(out,m,sizeof(float) * 16);
00080 }
00081 
00082 void m_identity(float *m) {
00083   m[0] = 1; m[4] = 0; m[8] = 0; m[12] = 0;
00084   m[1] = 0; m[5] = 1; m[9] = 0; m[13] = 0;
00085   m[2] = 0; m[6] = 0; m[10] = 1; m[14] = 0;
00086   m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
00087 }
00088 
00089 void m_multiply(const float *m1,const float *m2,float *out) {
00090   out[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
00091   out[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
00092   out[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
00093   out[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];
00094   out[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
00095   out[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
00096   out[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
00097   out[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];
00098   out[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
00099   out[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
00100   out[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
00101   out[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];
00102   out[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
00103   out[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
00104   out[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
00105   out[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];    
00106 }
00107 
00108 void m3_multiply(const float *m1,const float *m2,float *out) {
00109  out[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2];
00110   out[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2];
00111   out[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2];
00112   out[3] = 0;
00113   out[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6];
00114   out[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6];
00115   out[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6];
00116   out[7] =0;
00117   out[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10];
00118   out[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10];
00119   out[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10];
00120   out[11] = 0.f;
00121   out[12] = 0.f;
00122   out[13] = 0.f;
00123   out[14] = 0.f;
00124   out[15] = 1.f;
00125 
00126 }
00127 
00128 int m_inverse(const float *m,float *out) {
00129   float   det;
00130   det = m[0] * m[5] * m[10];
00131   det += m[4] * m[9] * m[2];
00132   det += m[8] * m[1] * m[6];
00133   det -= m[8] * m[5] * m[2];
00134   det -= m[4] * m[1] * m[10];
00135   det -= m[0] * m[9] * m[6];
00136   if(det * det < 1e-6) return -1;
00137   det = 1.0f / det;    
00138   out[0] =    (m[5] * m[10] - m[9] * m[6]) * det;
00139   out[1] =  - (m[1] * m[10] - m[9] * m[2]) * det;
00140   out[2] =    (m[1] * m[6] -  m[5] * m[2]) * det;
00141   out[3] = 0.0;
00142   out[4] =  - (m[4] * m[10] - m[8] * m[6]) * det;
00143   out[5] =    (m[0] * m[10] - m[8] * m[2]) * det;
00144   out[6] =  - (m[0] * m[6] -  m[4] * m[2]) * det;
00145   out[7] = 0.0;
00146   out[8] =    (m[4] * m[9] -  m[8] * m[5]) * det;
00147   out[9] =  - (m[0] * m[9] -  m[8] * m[1]) * det;
00148   out[10] =   (m[0] * m[5] -  m[4] * m[1]) * det;
00149   out[11] = 0.0;
00150   out[12] = - (m[12] * out[0] + m[13] * out[4] + m[14] * out[8]);
00151   out[13] = - (m[12] * out[1] + m[13] * out[5] + m[14] * out[9]);
00152   out[14] = - (m[12] * out[2] + m[13] * out[6] + m[14] * out[10]);
00153   out[15] = 1.0;
00154   return 0;
00155 }
00156 
00157 void m_transpose(const float *m,float *out) {
00158   out[0] = m[0]; out[4] = m[1]; out[8] = m[2]; out[12] = m[3];
00159   out[1] = m[4]; out[5] = m[5]; out[9] = m[6]; out[13] = m[7];
00160   out[2] = m[8]; out[6] = m[9]; out[10] = m[10]; out[14] = m[11];
00161   out[3] = m[12]; out[7] = m[13]; out[11] = m[14]; out[15] = m[15];
00162 }
00163 
00164 void m_transpose_rotation(const float *m,float *out) {
00165   out[0] = m[0]; out[4] = m[1]; out[8] = m[2]; out[12] = m[12];
00166   out[1] = m[4]; out[5] = m[5]; out[9] = m[6]; out[13] = m[13];
00167   out[2] = m[8]; out[6] = m[9]; out[10] = m[10]; out[14] = m[14];
00168   out[3] = m[3]; out[7] = m[7]; out[11] = m[11]; out[15] = m[15];
00169 }
00170 
00171 void m_rotation_x(float angle,float *out) {
00172   float rad = angle * ((float)deg2rad);
00173   float Cos = (float)cos(rad);
00174   float Sin = (float)sin(rad);
00175   out[0] = 1.0; out[4] = 0.0; out[8] = 0.0; out[12] = 0.0;
00176   out[1] = 0.0; out[5] = Cos; out[9] = -Sin; out[13] = 0.0;
00177   out[2] = 0.0; out[6] = Sin; out[10] = Cos; out[14] = 0.0;
00178   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00179 }
00180 
00181 void m_rotation_y(float angle,float *out) {
00182   float rad = angle * ((float)deg2rad);
00183   float Cos = (float)cos(rad);
00184   float Sin = (float)sin(rad);
00185   out[0] = Cos; out[4] = 0.0; out[8] = Sin; out[12] = 0.0;
00186   out[1] = 0.0; out[5] = 1.0; out[9] = 0.0; out[13] = 0.0;
00187   out[2] = -Sin; out[6] = 0.0; out[10] = Cos; out[14] = 0.0;
00188   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00189 }
00190 
00191 void m_rotation_z(float angle,float *out) {
00192   float rad = angle * ((float)deg2rad);
00193   float Cos = (float)cos(rad);
00194   float Sin = (float)sin(rad);
00195   out[0] = Cos; out[4] = -Sin; out[8] = 0.0; out[12] = 0.0;
00196   out[1] = Sin; out[5] =  Cos; out[9] = 0.0; out[13] = 0.0;
00197   out[2] = 0.0; out[6] = 0.0; out[10] = 1.0; out[14] = 0.0;
00198   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00199 }
00200 
00201 void m_rotation_v(float angle,float *v,float *out) {
00202   float rad = angle * ((float)deg2rad);
00203   float C = (float)cos(rad);
00204   float S = (float)sin(rad);
00205   float U = (1-C);
00206   out[0] = v[0]*v[0]*U +      C; out[4] = v[0]*v[1]*U - v[2]*S; out[8] = v[0]*v[2]*U + v[1]*S; out[12] = 0.0;
00207   out[1] = v[0]*v[1]*U + v[2]*S; out[5] = v[1]*v[1]*U +      C; out[9] = v[1]*v[2]*U - v[0]*S; out[13] = 0.0;
00208   out[2] = v[0]*v[2]*U - v[1]*S; out[6] = v[1]*v[2]*U + v[0]*S; out[10]= v[2]*v[2]*U +      C; out[14] = 0.0;
00209   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00210 }
00211 
00212 void m_rotation_x2(float angle,float *out) {
00213   float rad = angle;
00214   float Cos = (float)cos(rad);
00215   float Sin = (float)sin(rad);
00216   out[0] = 1.0; out[4] = 0.0; out[8] = 0.0; out[12] = 0.0;
00217   out[1] = 0.0; out[5] = Cos; out[9] = -Sin; out[13] = 0.0;
00218   out[2] = 0.0; out[6] = Sin; out[10] = Cos; out[14] = 0.0;
00219   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00220 }
00221 
00222 void m_rotation_y2(float angle,float *out) {
00223   float rad = angle;
00224   float Cos = (float)cos(rad);
00225   float Sin = (float)sin(rad); 
00226   out[0] = Cos; out[4] = 0.0; out[8] = Sin; out[12] = 0.0;
00227   out[1] = 0.0; out[5] = 1.0; out[9] = 0.0; out[13] = 0.0;
00228   out[2] = -Sin; out[6] = 0.0; out[10] = Cos; out[14] = 0.0;
00229   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00230 }
00231 
00232 void m_rotation_z2(float angle,float *out) {
00233   float rad = angle;
00234   float Cos = (float)cos(rad);
00235   float Sin = (float)sin(rad);
00236   out[0] = Cos; out[4] = -Sin; out[8] = 0.0; out[12] = 0.0;
00237   out[1] = Sin; out[5] = Cos; out[9] = 0.0; out[13] = 0.0;
00238   out[2] = 0.0; out[6] = 0.0; out[10] = 1.0; out[14] = 0.0;
00239   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00240 }
00241 
00242 void m_rotation_v2(float angle,float *v,float *out) {
00243   float rad = angle;
00244   float C = (float)cos(rad);
00245   float S = (float)sin(rad);
00246   float U = (1-C);
00247   out[0] = v[0]*v[0]*U +      C; out[4] = v[0]*v[1]*U - v[2]*S; out[8] = v[0]*v[2]*U + v[1]*S; out[12] = 0.0;
00248   out[1] = v[0]*v[1]*U + v[2]*S; out[5] = v[1]*v[1]*U +      C; out[9] = v[1]*v[2]*U - v[0]*S; out[13] = 0.0;
00249   out[2] = v[0]*v[2]*U - v[1]*S; out[6] = v[1]*v[2]*U + v[0]*S; out[10]= v[2]*v[2]*U +      C; out[14] = 0.0;
00250   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00251 }
00252 
00253 /*
00254   micha - phi rotation angle;
00255   theta: axe angle with horizontal plane Oxy
00256   psi: axe angle with sagital plane Oxz
00257 */
00258 
00259 void m_rotation_xyz(float phi, float theta, float psi, float *out)
00260 {
00261   phi   *= ((float)deg2rad);
00262   theta *= ((float)deg2rad);
00263   psi   *= ((float)deg2rad);
00264 
00265   out[0] =((float)(cos(theta)*cos(psi)));
00266   out[1] =((float)(cos(theta)*sin(psi)));
00267   out[2] =((float)(-sin(theta)));
00268   out[3] =0.0f;
00269   out[4] =((float)(sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi)));
00270   out[5] =((float)(sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi)));
00271   out[6] =((float)(sin(phi)*cos(theta)));
00272   out[7] =0.0f;
00273   out[8] =((float)(cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)));
00274   out[9] =((float)(cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)));
00275   out[10]=((float)(cos(phi)*cos(theta)));
00276   out[11]=0.0f;
00277   out[12]=0.0f;
00278   out[13]=0.0f;
00279   out[14]=0.0f;
00280   out[16]=1.0f;
00281 }
00282 
00283 /*
00284 void m_rotation_xyz(float phi, float theta, float psi, float *out)
00285 {
00286   phi *= deg2rad;
00287   theta *= deg2rad;
00288   psi *= deg2rad;
00289 
00290   out[0]=cos(theta)*cos(psi);                            out[4]=cos(theta)*sin(psi);                            out[8]=-sin(theta);          out[12]=0.0;
00291   out[1]=sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi); out[5]=sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi); out[9]=sin(phi)*cos(theta);  out[13]=0.0;
00292   out[2]=cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi); out[6]=cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi); out[10]=cos(phi)*cos(theta); out[14]=0.0;
00293   out[3]=0.0;                                            out[7]=0.0;                                            out[11]=0.0;                 out[15] = 1.0;
00294 }
00295 */
00296 
00297 void m_rotation_xyz2(float phi, float theta, float psi, float *out)
00298 {
00299   out[0] =((float)(cos(theta)*cos(psi)));
00300   out[4] =((float)(cos(theta)*sin(psi)));
00301   out[8] =((float)(-sin(theta)));
00302   out[12]=0.0f;
00303   out[1] =((float)(sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi)));
00304   out[5] =((float)(sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi)));
00305   out[9] =((float)(sin(phi)*cos(theta)));
00306   out[13]=0.0f;
00307   out[2] =((float)(cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)));
00308   out[6] =((float)(cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)));
00309   out[10]=((float)(cos(phi)*cos(theta)));
00310   out[14]=0.0f;
00311   out[3] =0.0f;
00312   out[7] =0.0f; 
00313   out[11]=0.0f;
00314   out[15]=1.0f;
00315 }
00316 
00317 void m_rotation_serial(float angle_1, float angle_2, float angle_3, float *out)
00318 {
00319   float Mtmp[16], Mtmp1[16], Mtmp2[16], Mtmp3[16];
00320   float vt1[]={0.0f,1.0f,0.0f};
00321   float vt2[]={1.0f,0.0f,0.0f};
00322   float vt3[]={0.0f,0.0f,1.0f};
00323   float v[3];
00324 
00325 
00326   //printf("l_sfe[deg]:%.2f\n",angle_1);
00327   //printf("l_saa[deg]:%.2f\n",angle_2);
00328   //printf("l_shr[deg]:%.2f\n\n",angle_3);
00329 
00330   m_rotation_v(angle_1, vt1, Mtmp1);
00331   v_transform(vt2, Mtmp1, v);
00332   m_rotation_v(angle_2, v, Mtmp2); 
00333   m_multiply(Mtmp2, Mtmp1, Mtmp);
00334   v_transform(vt3, Mtmp, v);
00335   m_rotation_v(angle_3, v, Mtmp3);
00336 
00337   m_multiply(Mtmp3, Mtmp, out);
00338 }
00339 
00340 void m_translate(const float *v,float *out) {
00341   out[0] = 1.0f; out[4] = 0.0f; out[8]  = 0.0f; out[12] = v[0];
00342   out[1] = 0.0f; out[5] = 1.0f; out[9]  = 0.0f; out[13] = v[1];
00343   out[2] = 0.0f; out[6] = 0.0f; out[10] = 1.0f; out[14] = v[2];
00344   out[3] = 0.0f; out[7] = 0.0f; out[11] = 0.0f; out[15] = 1.0f;
00345 }
00346 
00347 void m_scale(const float *scale,float *out) {
00348   out[0] = scale[0]; out[4] = 0.0; out[8] = 0.0; out[12] = 0.0;
00349   out[1] = 0.0; out[5] = scale[1]; out[9] = 0.0; out[13] = 0.0;
00350   out[2] = 0.0; out[6] = 0.0; out[10] = scale[2]; out[14] = 0.0;
00351   out[3] = 0.0; out[7] = 0.0; out[11] = 0.0; out[15] = 1.0;
00352 }
00353 
00354 void m_look_at(const float *eye,const float *dir,const float *up,float *out) {
00355   float x[3],y[3],z[3],ieye[3],m1[16],m2[16];
00356   v_sub(eye,dir,z);
00357   v_normalize(z,z);
00358   v_cross(up,z,x);
00359   v_cross(z,x,y);
00360   v_normalize(x,x);
00361   v_normalize(y,y);
00362   m1[0] = x[0]; m1[4] = x[1]; m1[8] = x[2]; m1[12] = 0.0;
00363   m1[1] = y[0]; m1[5] = y[1]; m1[9] = y[2]; m1[13] = 0.0;
00364   m1[2] = z[0]; m1[6] = z[1]; m1[10] = z[2]; m1[14] = 0.0;
00365   m1[3] = 0.0; m1[7] = 0.0; m1[11] = 0.0; m1[15] = 1.0;
00366   v_scale(eye,-1,ieye);
00367   m_translate(ieye,m2);
00368   m_multiply(m1,m2,out);
00369 }
00370 
00371 void m_shadow(const float *plane,const float *light,float *out) {
00372   float dot;
00373   dot = plane[0] * light[0] + plane[1] * light[1] +
00374     plane[2] * light[2] + plane[3] * light[3];
00375   out[0] = -light[0] * plane[0] + dot;
00376   out[1] = -light[1] * plane[0];
00377   out[2] = -light[2] * plane[0];
00378   out[3] = -light[3] * plane[0];
00379   out[4] = -light[0] * plane[1];
00380   out[5] = -light[1] * plane[1] + dot;
00381   out[6] = -light[2] * plane[1];
00382   out[7] = -light[3] * plane[1];
00383   out[8] = -light[0] * plane[2];
00384   out[9] = -light[1] * plane[2];
00385   out[10] = -light[2] * plane[2] + dot;
00386   out[11] = -light[3] * plane[2];
00387   out[12] = -light[0] * plane[3];
00388   out[13] = -light[1] * plane[3];
00389   out[14] = -light[2] * plane[3];
00390   out[15] = -light[3] * plane[3] + dot;
00391 }
00392 
00393 void q_set(const float *dir,float angle,float *out) {
00394   float sinangle,length,ilength;
00395   length = (float)sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
00396   if(length != 0.0) {
00397     ilength = 1.0f / length;
00398     sinangle = (float)sin(angle/2);
00399     out[0] = dir[0] * ilength * sinangle;
00400     out[1] = dir[1] * ilength * sinangle;
00401     out[2] = dir[2] * ilength * sinangle;
00402     out[3] = (float)cos(angle/2);
00403     return;
00404   }
00405   out[0] = out[1] = out[2] = 0.0;
00406   out[3] = 1.0;
00407 }
00408 
00409 void q_copy(const float *q,float *out) {
00410   out[0] = q[0];
00411   out[1] = q[1];
00412   out[2] = q[2];
00413   out[3] = q[3];
00414 }
00415 
00416 void q_slerp(const float *q1,const float *q2,float t,float *out) {
00417   float omega,cosomega,sinomega,k1,k2,q[4];
00418   cosomega = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
00419   if(cosomega < 0.0) {
00420     cosomega = -cosomega;
00421     q[0] = -q2[0];
00422     q[1] = -q2[1];
00423     q[2] = -q2[2];
00424     q[3] = -q2[3];
00425   } else {
00426     q[0] = q2[0];
00427     q[1] = q2[1];
00428     q[2] = q2[2];
00429     q[3] = q2[3];
00430   }
00431   if(1.0 - cosomega > 1e-6) {
00432     omega = (float)acos(cosomega);
00433     sinomega = (float)sin(omega);
00434     k1 = (float)(sin((1.0 - t) * omega) / sinomega);
00435     k2 = (float)(sin(t * omega) / sinomega);
00436   } else {
00437     k1 = 1.0f - t;
00438     k2 = t;
00439   }
00440   out[0] = q1[0] * k1 + q[0] * k2;
00441   out[1] = q1[1] * k1 + q[1] * k2;
00442   out[2] = q1[2] * k1 + q[2] * k2;
00443   out[3] = q1[3] * k1 + q[3] * k2;
00444 }
00445 //first q1 then q2
00446 void q_multiply(const float *q1,const float *q2,float *out) {
00447   out[0] = q1[3] * q2[0] + q1[0] * q2[3] - q1[1] * q2[2] + q1[2] * q2[1];
00448   out[1] = q1[3] * q2[1] + q1[1] * q2[3] + q1[0] * q2[2] - q1[2] * q2[0];
00449   out[2] = q1[3] * q2[2] + q1[2] * q2[3] - q1[0] * q2[1] + q1[1] * q2[0];
00450   out[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
00451 }
00452 
00453 
00454 void q_v_mult(const float *q, const CVector3_t in, CVector3_t out){
00455   CVector3_t v1,v2,v3;
00456   float f1;
00457   f1 = q[3]*q[3];
00458   v_scale(in,f1,v1); //alpha^2*v
00459   v_cross(q,in,v2); //beta x v
00460   v_scale(v2,2*q[3],v3); // 2*alpha*(beta x v)
00461   v_add(v1,v3,v3); // alpha^2*v+2*alpha*(beta x v)
00462   f1 = v_dot(q,in); //beta'*v
00463   v_scale(q,f1,v1); //(beta'*v)*beta 
00464   v_add(v3,v1,v3);// alpha^2*v+2*alpha*(beta x v)+(beta'*v)*beta 
00465   v_cross(v2,q,v1); //(beta x v) x beta
00466   v_sub(v3,v1,out); 
00467 }
00468 // return rotation axis and angle
00469 // implements the formulae 3.3.18 and 3.3.19 (p.73) of Altmann, 
00470 // "Rotations, quaternions and double groups", 1986
00471 float m_rotation_axis(const CMatrix3_t m,CVector3_t out){
00472   float angle,f;
00473   f =  0.5*(m[0]+m[5]+m[10]-1);
00474   if(abs(f)>1){
00475     return 0;
00476   }
00477   else{
00478     angle = acos(f);
00479     if(abs(angle)<epsilon){
00480       v_clear(out);
00481       return 0.0f;
00482     }
00483     else{
00484       out[0] = m[6]-m[9];
00485       out[1] = m[8]-m[2];
00486       out[2] = m[1]-m[4];
00487       f = 1/(2*sin(angle));
00488       v_scale(out,f,out);
00489       return angle;
00490     }
00491   }
00492 }
00493 
00494 
00495 
00496 void q_to_matrix(const float *q,float *out) {
00497   float x2,y2,z2,xx,yy,zz,xy,yz,xz,wx,wy,wz;
00498   x2 = q[0] + q[0];
00499   y2 = q[1] + q[1];
00500   z2 = q[2] + q[2];
00501   xx = q[0] * x2;
00502   yy = q[1] * y2;
00503   zz = q[2] * z2;
00504   xy = q[0] * y2;
00505   yz = q[1] * z2;
00506   xz = q[2] * x2;
00507   wx = q[3] * x2;
00508   wy = q[3] * y2;
00509   wz = q[3] * z2;
00510   out[0] = 1.0f - (yy + zz);
00511   out[1] = xy + wz;
00512   out[2] = xz - wy;
00513   out[3] = 0.0f;
00514   out[4] = xy - wz;
00515   out[5] = 1.0f - (xx + zz);
00516   out[6] = yz + wx;
00517   out[7] = 0.0f;
00518   out[8] = xz + wy;
00519   out[9] = yz - wx;
00520   out[10] = 1.0f - (xx + yy);
00521   out[11] = 0.0f;
00522   out[12] = 0.0f;
00523   out[13] = 0.0f;
00524   out[14] = 0.0f;
00525   out[15] = 1.0f;
00526 }
00527 
00528 void q_clear(CQuat_t q) {
00529   v_clear(q);
00530   q[3]=1.0f;
00531 }
00532 
00533 void q_complete(const CVector3_t v, CQuat_t out){
00534   v_copy(v,out);
00535   out[3] = sqrt(1-v_squ_length(v));
00536 }
00537 
00538 
00539 #ifdef DEPRECATED
00540 
00544 void q_derivative(const CQuat_t q, CQuat_t out){
00545  
00546   float n = v_normalize(q,out);
00547   
00548   if(q[3]>epsilon){
00549       out[3] = -n/q[3];
00550   }
00551   else{
00552     v_normalize(q,out) ;
00553     out[3]=0; // discontinuity, 'm'
00554   }
00555 }
00556 
00557 #endif
00558 
00559 /* functions added by M. Hersch */
00560 #ifdef M_MACROS
00561 void v_cross(const float *v1,const float *v2,float *out) {
00562   out[0] = v1[1] * v2[2] - v1[2] * v2[1];
00563   out[1] = v1[2] * v2[0] - v1[0] * v2[2];
00564   out[2] = v1[0] * v2[1] - v1[1] * v2[0];
00565 }
00566 
00567 void v4_sub(CVector4_t a, CVector4_t b, CVector4_t c){
00568   c[0] = a[0] - b[0]; 
00569   c[1] = a[1] - b[1]; 
00570   c[2] = a[2] - b[2]; 
00571   c[3] = a[3] - b[3];
00572 }
00573 
00574 void v4_addv(CVector4_t a, CVector4_t b, CVector4_t c){
00575   c[0] = a[0] + b[0]; 
00576   c[1] = a[1] + b[1]; 
00577   c[2] = a[2] + b[2]; 
00578   c[3] = a[3] + b[3];
00579 }
00580 
00581 void v4_scale(CVector4_t a, float b, CVector4_t c){
00582   c[0] = a[0] * b;   c[1] = a[1] * b;   c[2] = a[2] * b; c[3] = a[3] * b;
00583 }
00584 
00585 void v4_copy(CVector4_t a, CVector4_t b){
00586   b[0] = a[0];  b[1] = a[1]; b[2] = a[2]; b[3] = a[3];
00587 }
00588 
00589 #endif
00590 
00591 
00592 float v4_length(const CVector4_t v) {
00593   return (float)(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]));
00594 }
00595 
00596 float v_squ_length(const CVector3_t v){
00597   return (float)(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
00598 }
00599 
00600 float v4_squ_length(const CVector4_t v){
00601   return (float)(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
00602 }
00603 /* out = m*v*m' where m = 3x4 indexed in a transposed way (0 1 2 3// 5 ...), 
00604 v is a diagonal matrix of size 4*/
00605 
00606 // out = m'm ; m = 3 rows, 4 columns
00607 void m3_4_square(CMatrix4_t m, CMatrix3_t out){
00608   //for correct matrix index
00609   out[0]= m[0]*m[0] + m[4]*m[4] + m[8]*m[8] + m[12]*m[12];
00610   out[1]= m[0]*m[1] + m[4]*m[5] + m[8]*m[9] + m[12]*m[13];
00611   out[2]= m[0]*m[2] + m[4]*m[6] + m[8]*m[10]+ m[12]*m[14];
00612   out[3]= 0.0f;
00613   out[4]= out[1];
00614   out[5]= m[1]*m[1] + m[5]*m[5] + m[9]*m[9] + m[13]*m[13];
00615   out[6]= m[1]*m[2] + m[5]*m[6] + m[9]*m[10]+ m[13]*m[14];
00616   out[7]= 0.0f;
00617   out[8]= out[2];
00618   out[9]= out[6];
00619   out[10]=m[2]*m[2] + m[6]*m[6] + m[10]*m[10]+ m[14]*m[14];
00620   out[11]=0.0f;
00621   out[12]=0.0f;
00622   out[13]=0.0f;
00623   out[14]=0.0f;
00624   out[15]=0.0f;    
00625 }
00626 
00627 
00628 
00629 
00630 
00631 void m3_4_weighted_square(CMatrix4_t m,CVector4_t v, CMatrix3_t out)
00632 {
00633 #ifndef CORRECT_MATRIX_INDEX
00634   out[0]= v[0]*m[0]*m[0] + v[1]*m[1]*m[1] + v[2]*m[2]*m[2] + v[3]*m[3]*m[3];
00635   out[1]= v[0]*m[0]*m[4] + v[1]*m[1]*m[5] + v[2]*m[2]*m[6] + v[3]*m[3]*m[7];
00636   out[2]= v[0]*m[0]*m[8] + v[1]*m[1]*m[9] + v[2]*m[2]*m[10]+ v[3]*m[3]*m[11];
00637   out[3]= 0.0f;
00638   out[4]= out[1];
00639   out[5]= v[0]*m[4]*m[4] + v[1]*m[5]*m[5] + v[2]*m[6]*m[6] + v[3]*m[7]*m[7];
00640   out[6]= v[0]*m[4]*m[8] + v[1]*m[5]*m[9] + v[2]*m[6]*m[10]+ v[3]*m[7]*m[11];
00641   out[7]= 0.0f;
00642   out[8]= out[2];
00643   out[9]= out[6];
00644   out[10]= v[0]*m[8]*m[8] + v[1]*m[9]*m[9]+ v[2]*m[10]*m[10]+ v[3]*m[11]*m[11];
00645   out[11]=0.0f;
00646   out[12]=0.0f;
00647   out[13]=0.0f;
00648   out[14]=0.0f;
00649   out[15]=0.0f;    
00650 #else
00651   out[0]= v[0]*m[0]*m[0] + v[1]*m[4]*m[4] + v[2]*m[8]*m[8] + v[3]*m[12]*m[12];
00652   out[1]= v[0]*m[0]*m[1] + v[1]*m[4]*m[5] + v[2]*m[8]*m[9] + v[3]*m[12]*m[13];
00653   out[2]= v[0]*m[0]*m[2] + v[1]*m[4]*m[6] + v[2]*m[8]*m[10]+ v[3]*m[12]*m[14];
00654   out[3]= 0.0f;
00655   out[4]= out[1];
00656   out[5]= v[0]*m[1]*m[1] + v[1]*m[5]*m[5] + v[2]*m[9]*m[9] + v[3]*m[13]*m[13];
00657   out[6]= v[0]*m[1]*m[2] + v[1]*m[5]*m[6] + v[2]*m[9]*m[10]+ v[3]*m[13]*m[14];
00658   out[7]= 0.0f;
00659   out[8]= out[2];
00660   out[9]= out[6];
00661   out[10]= v[0]*m[2]*m[2] + v[1]*m[6]*m[6]+ v[2]*m[10]*m[10]+ v[3]*m[14]*m[14];
00662   out[11]=0.0f;
00663   out[12]=0.0f;
00664   out[13]=0.0f;
00665   out[14]=0.0f;
00666   out[15]=0.0f;    
00667 
00668 #endif
00669 
00670 }
00671 void m3_4_t_weighted_square(CMatrix4_t m,CVector3_t v, CMatrix4_t out)
00672 {
00673 #ifndef CORRECT_MATRIX_INDEX
00674   out[0]=  v[0]*m[0]*m[0] + v[1]*m[4]*m[4] + v[2]*m[8]*m[8];
00675   out[1]=  v[0]*m[0]*m[1] + v[1]*m[4]*m[5] + v[2]*m[8]*m[9];
00676   out[2]=  v[0]*m[0]*m[2] + v[1]*m[4]*m[6] + v[2]*m[8]*m[10];
00677   out[3]=  v[0]*m[0]*m[3] + v[1]*m[4]*m[7] + v[2]*m[8]*m[11];
00678   out[4]= out[1];
00679   out[5]=  v[0]*m[1]*m[1] + v[1]*m[5]*m[5] + v[2]*m[9]*m[9];
00680   out[6]=  v[0]*m[1]*m[2] + v[1]*m[5]*m[6] + v[2]*m[9]*m[10];
00681   out[7]=  v[0]*m[1]*m[3] + v[1]*m[5]*m[7] + v[2]*m[9]*m[11];
00682   out[8]= out[2];
00683   out[9]= out[6];
00684   out[10]= v[0]*m[2]*m[2] + v[1]*m[6]*m[6] + v[2]*m[10]*m[10];
00685   out[11]= v[0]*m[2]*m[3] + v[1]*m[6]*m[7] + v[2]*m[10]*m[11];
00686   out[12]= out[3];
00687   out[13]= out[7];
00688   out[14]= out[11];
00689   out[15]= v[0]*m[3]*m[3] + v[1]*m[7]*m[7] + v[2]*m[11]*m[11];   
00690 #else
00691   out[0]=  v[0]*m[0]*m[0] + v[1]*m[1]*m[1] + v[2]*m[2]*m[2];
00692   out[1]=  v[0]*m[0]*m[4] + v[1]*m[1]*m[5] + v[2]*m[2]*m[6];
00693   out[2]=  v[0]*m[0]*m[8] + v[1]*m[1]*m[9] + v[2]*m[2]*m[10];
00694   out[3]=  v[0]*m[0]*m[12]+ v[1]*m[1]*m[13]+ v[2]*m[2]*m[14];
00695   out[4]= out[1];
00696   out[5]=  v[0]*m[4]*m[4] + v[1]*m[5]*m[5] + v[2]*m[6]*m[6];
00697   out[6]=  v[0]*m[4]*m[8] + v[1]*m[5]*m[9] + v[2]*m[6]*m[10];
00698   out[7]=  v[0]*m[4]*m[12]+ v[1]*m[5]*m[13]+ v[2]*m[6]*m[14];
00699   out[8]= out[2];
00700   out[9]= out[6];
00701   out[10]= v[0]*m[8]*m[8] + v[1]*m[9]*m[9] + v[2]*m[10]*m[10];
00702   out[11]= v[0]*m[8]*m[12]+ v[1]*m[9]*m[13]+ v[2]*m[10]*m[14];
00703   out[12]= out[3];
00704   out[13]= out[7];
00705   out[14]= out[11];
00706   out[15]= v[0]*m[12]*m[12]+ v[1]*m[13]*m[13]+ v[2]*m[14]*m[14];   
00707 #endif
00708 }
00709 
00710 
00711 void inverse_diag3(CVector3_t v, CVector3_t out)
00712 {
00713   int i;
00714   for(i=0;i<3;i++){
00715     out[i]=1.0/v[i];
00716   }
00717 }
00718 
00719 void inverse_diag4(CVector4_t v, CVector4_t out)
00720 {
00721   int i;
00722   for(i=0;i<4;i++){
00723     out[i]=1.0/v[i];
00724   }
00725 }
00726 
00727 
00728 void m3_add_diag(CMatrix3_t m, CVector3_t v, CMatrix3_t out)
00729 {
00730   m_copy(m,out);
00731   out[0] += v[0];
00732   out[5] += v[1];
00733   out[10] += v[2];
00734 }
00735 
00736 void m4_add_diag(CMatrix4_t m, CVector4_t v, CMatrix4_t out)
00737 {
00738   m4_copy(m,out);
00739   out[0] += v[0];
00740   out[5] += v[1];
00741   out[10]+= v[2];
00742   out[15]+= v[3];
00743 }
00744 
00745 
00746 /* out = m*v, where m = 3x4, v=4x1 */
00747 void m3_4_v_multiply(CMatrix4_t m, CVector4_t v, CVector3_t out)
00748 {
00749 #ifndef CORRECT_MATRIX_INDEX
00750   out[0]= m[0]*v[0] + m[1]*v[1] + m[2]*v[2] + m[3]*v[3];
00751   out[1]= m[4]*v[0] + m[5]*v[1] + m[6]*v[2] + m[7]*v[3];
00752   out[2]= m[8]*v[0] + m[9]*v[1] + m[10]*v[2]+ m[11]*v[3];
00753 #else
00754   out[0]= m[0]*v[0] + m[4]*v[1] + m[8]*v[2] + m[12]*v[3];
00755   out[1]= m[1]*v[0] + m[5]*v[1] + m[9]*v[2] + m[13]*v[3];
00756   out[2]= m[2]*v[0] + m[6]*v[1] + m[10]*v[2]+ m[14]*v[3];
00757 #endif
00758 }
00759 /* out = m*v , where m is 4x3, v is 3x1 out is 4x1*/
00760 void m4_3_v_multiply(CMatrix4_t m, CVector3_t v1, CVector4_t out){
00761   // correct matrix index
00762   out[0] = v1[0] * m[0] + v1[1] * m[4] + v1[2] * m[8];
00763   out[1] = v1[0] * m[1] + v1[1] * m[5] + v1[2] * m[9];
00764   out[2] = v1[0] * m[2] + v1[1] * m[6] + v1[2] * m[10];
00765   out[3] = v1[0] * m[3] + v1[1] * m[7] + v1[2] * m[11];
00766 }
00767 
00768 
00769 /* out = m*v, where m is 3x3 diagonal,v=3x1 */ 
00770 void m3_diag_v_multiply(CVector3_t m, CVector3_t v, CVector3_t out)
00771 {
00772   out[0]= m[0]*v[0];
00773   out[1]= m[1]*v[1];
00774   out[2]= m[2]*v[2];
00775 }
00776 /* out = m'*v, where m=3x4, v=3x1 */
00777 
00778 
00779 void m3_4_t_v_multiply(CMatrix4_t m, CVector3_t v, CVector4_t out)
00780 {
00781 #ifndef CORRECT_MATRIX_INDEX
00782   out[0] = m[0]*v[0] + m[4]*v[1] + m[8]*v[2];
00783   out[1] = m[1]*v[0] + m[5]*v[1] + m[9]*v[2];
00784   out[2] = m[2]*v[0] + m[6]*v[1] + m[10]*v[2];
00785   out[3] = m[3]*v[0] + m[7]*v[1] + m[11]*v[2];
00786 #else
00787   out[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
00788   out[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2];
00789   out[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2];
00790   out[3] = m[12]*v[0] + m[13]*v[1] + m[14]*v[2];
00791 #endif
00792 }
00793 //out = m1'*m2 where  m1 is 3x4, m2 is 3x3, out is 4x3
00794 void m3_4_t_m_multiply(CMatrix4_t m1, CMatrix3_t m2, CMatrix4_t out){
00795   for(int i=0;i<3;i++){
00796     m3_4_t_v_multiply(m1,m2+4*i,out+4*i);
00797   }
00798 }
00799 
00800 void m4_4_v_multiply(CMatrix4_t m ,CVector4_t v1, CVector4_t out)
00801 {
00802   out[0] = v1[0] * m[0] + v1[1] * m[4] + v1[2] * m[8] + v1[3] * m[12];
00803   out[1] = v1[0] * m[1] + v1[1] * m[5] + v1[2] * m[9] + v1[3] * m[13];
00804   out[2] = v1[0] * m[2] + v1[1] * m[6] + v1[2] * m[10]+ v1[3] * m[14];
00805   out[3] = v1[0] * m[3] + v1[1] * m[7] + v1[2] * m[11]+ v1[3] * m[15];
00806 }
00807 
00808 
00809 /* out = m*v, where m is 4x4 diagonal,v=4x1 */ 
00810 void m4_diag_v_multiply(CVector4_t m, CVector4_t v, CVector4_t out)
00811 {
00812   out[0]= m[0]*v[0];
00813   out[1]= m[1]*v[1];
00814   out[2]= m[2]*v[2];
00815   out[3]= m[3]*v[3];
00816 }
00817 
00818 void m4_minus(CMatrix4_t m, CMatrix4_t out)
00819 {
00820   for (int i=0;i<16;i++){
00821     out[i] = -m[i];
00822   }
00823 }
00824 
00825 // indexed (0 1 2 3 // 4 5 ...)
00826 void v_mult(const CVector3_t v1,const CVector3_t v2, CMatrix4_t m){
00827 #ifndef CORRECT_MATRIX_INDEX
00828   for(int i=0;i<3;i++){
00829      for(int j=0;j<3;j++){
00830        m[i*4+j]=v1[i]*v2[j];
00831      }
00832      m[i*4+3] = v1[i];
00833   }
00834  v_copy(v2,&(m[12]));
00835 #else
00836  for(int i=0;i<3;i++){
00837      for(int j=0;j<3;j++){
00838        m[i*4+j]=v2[i]*v1[j];
00839      }
00840      m[i*4+3] = v2[i];
00841   }
00842  v_copy(v1,&(m[12]));
00843 #endif
00844 
00845   m[15] = 1;
00846 }
00847 
00848 
00849 
00850 void m_set_v3_column(const CVector3_t v,int i, CMatrix3_t out)
00851 {
00852     v_copy(v,out+4*i);    
00853 }
00854 
00855 void m_get_v3_column(const CMatrix4_t m,int i, CVector3_t out)
00856 {
00857     v_copy(m+4*i,out);    
00858 }
00859 
00860 void m_clear(CMatrix3_t m){
00861    for(int i=0;i<15;i++){
00862      m[i]=0;
00863    }
00864    m[15]=1;
00865 }
00866 
00867 // uses method described p.648 of Zwillinger
00868 float normalSample(float mean, float var)
00869 {
00870   float v1,v2,r,x;
00871   r=2;
00872   while(r>1 || r==0.0){
00873     v1 = rand()/((float)RAND_MAX);
00874     v2 = rand()/((float)RAND_MAX);
00875     r= v1*v1+v2*v2;
00876   }
00877   x=v1*sqrt(-2*log(r)/r);
00878   return x*sqrt(var)+mean;
00879 }
00880 
00881 float get_angle(float c,float s){
00882   //  cout<<c<<" "<<s<<endl;
00883   if(abs(s)<epsilon){
00884     return s;
00885   }
00886   return sign(s)*acos(c);
00887 }
00888 
00889 
00890 //(((s)<0)?-acos(c):acos(c))//c=cos, s=sin
00891 
00892 ostream& operator<<(ostream& out, const CVector3_t& v)
00893 {
00894   out << v[0] <<" "<<v[1] <<" "<<v[2];
00895   return out;
00896 }
00897 
00898 ostream& operator<<(ostream& out, const CVector4_t& v)
00899 {
00900   out << v[0] <<" "<<v[1] <<" "<<v[2]<<" "<<v[3];
00901   return out;
00902 }
00903 
00904 // ostream& operator << (ostream& out, const CMatrix3_t& m)
00905 // {
00906  
00907 //   out << m[0] << '\t'<< m[1] << '\t'<< m[2] <<endl
00908 //       << m[4] << '\t'<< m[5] << '\t'<< m[6] <<endl
00909 //       << m[8] << '\t'<< m[9] << '\t'<< m[10] <<endl;
00910 //   return out;
00911 // }
00912 
00913 ostream& operator<<(ostream& out, const CMatrix4_t& m)
00914 {
00915 #ifndef CORRECT_MATRIX_INDEX
00916   out << m[0] << '\t'<< m[1] << '\t'<< m[2]<< '\t'<< m[3] <<endl
00917       << m[4] << '\t'<< m[5] << '\t'<< m[6]<< '\t'<< m[7] <<endl
00918       << m[8] << '\t'<< m[9] << '\t'<< m[10]<< '\t'<< m[11] <<endl
00919       << m[12] << '\t'<< m[13] << '\t'<< m[14]<< '\t'<< m[15] <<endl;
00920 #else
00921   out << m[0] << '\t'<< m[4] << '\t'<< m[8]<< '\t'<< m[12] <<endl
00922       << m[1] << '\t'<< m[5] << '\t'<< m[9]<< '\t'<< m[13] <<endl
00923       << m[2] << '\t'<< m[6] << '\t'<< m[10]<< '\t'<< m[14] <<endl
00924       << m[3] << '\t'<< m[7] << '\t'<< m[11]<< '\t'<< m[15] <<endl;
00925 #endif
00926   return out;
00927 }
00928 
00929 
00930 istream& operator>>(istream& in,CVector3_t& v){
00931    in>>v[0]>>v[1]>>v[2];
00932    return in;
00933 }
00934 
00935 
00936 int segments_nearest_points(const CVector3_t p1,const CVector3_t vec1,
00937                             const CVector3_t p2,const CVector3_t vec2, 
00938                             float *nearest_point1, float *nearest_point2,
00939                             float *dist){
00940   CMatrix3_t mat;
00941   CMatrix3_t invmat;
00942   CVector3_t vec3,tmp,tmp1,tmp2,k,v2;
00943   int i; 
00944   float f;
00945   m_identity(mat);
00946   v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not //
00947   f= v_length(vec3)/(v_length(vec1)*v_length(vec2));//sine of angle
00948   if(f<epsilon){
00949     //    cout<<"aborting"<<endl;
00950     return 0;
00951   }
00952 
00953   v_scale(vec2,-1,v2);
00954   m_set_v3_column(vec1,0,mat);
00955   m_set_v3_column(v2,1,mat);
00956   m_set_v3_column(vec3,2,mat);
00957   m_inverse(mat,invmat);
00958   v_sub(p2,p1,tmp);
00959   v_transform_normal(tmp,invmat,k);
00960   for(i=0;i<2;i++){
00961     k[i] = max(min(1,k[i]),0);
00962   }
00963   v_scale(vec1,k[0],tmp);
00964   v_add(p1,tmp,tmp1);//cout<<"tmp1= "<<tmp1<<endl;
00965   v_scale(vec2,k[1],tmp);
00966   v_add(p2,tmp,tmp2);//cout<<"tmp2= "<<tmp2<<endl;
00967   v_sub(tmp2,tmp1,tmp);
00968 
00969   *dist=v_length(tmp);
00970   *nearest_point1 = k[0];//cout<<"k0= "<<k[0];
00971   *nearest_point2 = k[1];//cout<<"  k1= "<<k[1]<<endl;
00972   return 1;
00973 }
 All Data Structures Functions Variables

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