mathlib.h

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 
00025 #ifndef __MATHLIB_H__
00026 #define __MATHLIB_H__
00027 
00028 
00029 #include <math.h>
00030 #include <float.h>
00031 #include <iostream>
00032 #include <fstream>
00033 #include <stdlib.h>
00034 #include <string.h>
00035 
00036 using namespace std;
00037 
00038 #ifndef CORRECT_MATRIX_INDEX
00039 #define CORRECT_MATRIX_INDEX
00040 #endif
00041 
00042 typedef float vec2_t[2];
00043 typedef float vec3_t[3];
00044 typedef float vec4_t[4];
00045 typedef float matrix_t[16];
00046 
00047 typedef vec3_t   CVector3_t;
00048 typedef vec4_t   CVector4_t; // added M.Hersch
00049 typedef vec4_t   CQuat_t;
00050 typedef matrix_t CMatrix3_t;
00051 typedef matrix_t CMatrix4_t;
00052 
00053 #define pi 3.14159265358979323846
00054 #define deg2rad (pi / 180.0)
00055 #define rad2deg (180.0 / pi)
00056 #define epsilon 1e-6
00057 
00058 #ifndef RND
00059 #define RND(x)  ((x)*((double)rand())/((double)(RAND_MAX+1.0)))
00060 #endif
00061 #define coutvec(v)  cout <<(v)[0]<<" "<<(v)[1]<<" "<<(v)[2]<<endl
00062 #define coutvec4(v) cout <<(v)[0]<<" "<<(v)[1]<<" "<<(v)[2]<<" "<<(v)[3]<<endl
00063 #define min(a,b) ((a)<(b)?(a):(b))
00064 #define max(a,b) ((a)>(b)?(a):(b))
00065 
00066 
00067 #define sign(x) ( (x)>=0? 1:-1 )
00068 #define abs(x) ((x)>=0?(x):(-(x)))
00069 //#define get_angle(c,s)(((s)<0)?-acos(c):acos(c))//c=cos, s=sin
00070 
00071 
00072 
00073 #define v_set(x,y,z,v) { (v)[0] = x; (v)[1] = y; (v)[2] = z; }
00074 #define v_clear(v) { (v)[0] = (v)[1] = (v)[2] = 0; }
00075 #define v_copy(a,b) { (b)[0] = (a)[0]; (b)[1] = (a)[1]; (b)[2] = (a)[2]; }
00076 #define v_scale(a,b,c) { \
00077         (c)[0] = (a)[0] * (b); \
00078         (c)[1] = (a)[1] * (b); \
00079         (c)[2] = (a)[2] * (b); \
00080 }
00081 #define v_add(a,b,c) { \
00082         (c)[0] = (a)[0] + (b)[0]; \
00083         (c)[1] = (a)[1] + (b)[1]; \
00084         (c)[2] = (a)[2] + (b)[2]; \
00085 }
00086 #define v_sub(a,b,c) { \
00087         (c)[0] = (a)[0] - (b)[0]; \
00088         (c)[1] = (a)[1] - (b)[1]; \
00089         (c)[2] = (a)[2] - (b)[2]; \
00090 }
00091 #define v_dot(a,b) ((a)[0] * (b)[0] + (a)[1] * (b)[1] + (a)[2] * (b)[2])
00092 #define v_cross(a,b,c){ \
00093         (c)[0] = (a)[1] * (b)[2] - (a)[2] * (b)[1]; \
00094         (c)[1] = (a)[2] * (b)[0] - (a)[0] * (b)[2]; \
00095         (c)[2] = (a)[0] * (b)[1] - (a)[1] * (b)[0]; \
00096 }
00097 // added by M.Hersch
00098 
00099 #define v4_dot(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2] + (a)[3]*(b)[3])
00100 #define v4_set(x,y,z,u,v) { (v)[0] = x; (v)[1] = y; (v)[2] = z; (v)[3] = u; }
00101 
00102 #define v4_add(a,b,c) { \
00103         (c)[0] = (a)[0] + (b)[0]; \
00104         (c)[1] = (a)[1] + (b)[1]; \
00105         (c)[2] = (a)[2] + (b)[2]; \
00106         (c)[3] = (a)[3] + (b)[3]; \
00107 }
00108 
00109 #define m4_add(a,b,c) m_add((a),(b),(c))
00110 #define m4_sub(a,b,c) m_sub((a),(b),(c))
00111 
00112 #ifndef CORRECT_MATRIX_INDEX  
00113 #define m3_v_multiply(m,v,o) v_transform_normal2(v,m,o)
00114 #else
00115 #define m3_v_multiply(m,v,o) v_transform_normal(v,m,o)
00116 #endif
00117 
00118 #define m4_copy(m,o) (m_copy((m),(o)))
00119 
00120 
00121 #ifndef M_MACROS
00122 #define v4_sub(a,b,c) { \
00123         (c)[0] = (a)[0] - (b)[0]; \
00124         (c)[1] = (a)[1] - (b)[1]; \
00125         (c)[2] = (a)[2] - (b)[2]; \
00126         (c)[3] = (a)[3] - (b)[3]; \
00127 }
00128 #define v4_scale(a,b,c) { \
00129         (c)[0] = (a)[0] * (b); \
00130         (c)[1] = (a)[1] * (b); \
00131         (c)[2] = (a)[2] * (b); \
00132         (c)[3] = (a)[3] * (b); \
00133 }
00134 
00135 #define v4_add(a,b,c) { \
00136         (c)[0] = (a)[0] + (b)[0]; \
00137         (c)[1] = (a)[1] + (b)[1]; \
00138         (c)[2] = (a)[2] + (b)[2]; \
00139         (c)[3] = (a)[3] + (b)[3]; \
00140 }
00141 
00142 #define v4_copy(a,b) { (b)[0] = (a)[0]; (b)[1] = (a)[1]; (b)[2] = (a)[2]; \
00143 (b)[3] = (a)[3]; }
00144 
00145 #define v4_clear(a){(a)[0]=0;(a)[1]=0;(a)[2]=0;(a)[3]=0;}
00146 #else
00147 
00148 void v_cross(const float *v1,const float *v2,float *out);
00149 void v4_scale(CVector4_t a, float b, CVector4_t c);
00150 void v4_add(CVector4_t a, CVector4_t b, CVector4_t c);
00151 void v4_sub(CVector4_t a, CVector4_t b, CVector4_t c);
00152 void v4_copy(CVector4_t a, CVector4_t b);
00153 #endif
00154 
00155 
00156 #define q_inv(q1,q2)   {v_scale((q1),-1,(q2));(q2)[3]=(q1)[3];}
00157 
00158 
00159 float v_length(const float *v);
00160 float v_normalize(const float *v,float *out);
00161 void v_translate(const float *m,float *out);
00162 void v_transform(const float *v,const float *m,float *out);
00163 void v_transform_normal(const float *v,const float *m,float *out);
00164 void v_transform_normal2(const float *v,const float *m,float *out);
00165 
00166 // v4 vector operations (added by Micha Hersch)
00167 float v4_length(const CVector4_t v);
00168 float v4_squ_length(const CVector4_t v);
00169 float v_squ_length(const CVector3_t v);
00170 
00171 void inverse_diag3(CVector3_t v1,CVector3_t v2);
00172 void inverse_diag4(CVector4_t v1,CVector4_t v2);
00173 
00178 void m3_4_square(CMatrix4_t m, CMatrix3_t out);
00184 void m3_4_weighted_square(CMatrix4_t m,CVector4_t v, CMatrix3_t out);
00185 void m3_4_t_weighted_square(CMatrix4_t m,CVector3_t v, CMatrix4_t out);
00186 void m3_add_diag(CMatrix3_t m, CVector3_t v, CMatrix3_t out);
00187 void m4_add_diag(CMatrix4_t m, CVector4_t v, CMatrix4_t out);
00188 void m3_4_v_multiply(CMatrix4_t m, CVector4_t v, CVector3_t out);
00189 void m4_4_v_multiply(CMatrix4_t m, CVector4_t v, CVector4_t out);
00190 void m4_3_v_multiply(CMatrix4_t m, CVector3_t v, CVector4_t out);//out =
00191 void m3_diag_v_multiply(CVector3_t m, CVector3_t v, CVector3_t out);
00192 void m3_4_t_v_multiply(CMatrix4_t m, CVector3_t v, CVector4_t out);
00193 void m3_4_t_m_multiply(CMatrix4_t m1, CMatrix3_t m2, CVector4_t out);// out=m1'm2
00194 void m4_diag_v_multiply(CVector4_t m, CVector4_t v, CVector4_t out);
00195 void m4_minus(CMatrix4_t m, CMatrix4_t out);
00196 void m_set_v3_column(const CVector3_t v,int i,CMatrix4_t out);
00197 void m_get_v3_column(const CMatrix4_t m,int i,CVector3_t out);
00198 void m3_multiply(const float *m1,const float *m2,float *out); // 
00199 void v_mult(const CVector3_t v1,const CVector3_t v2, CMatrix4_t m);
00200 
00201   // matrix
00202 void m_copy(const float *m,float *out);
00203 void m_identity(float *m);
00204 void m_clear(CMatrix3_t m);//sets elmt 15 to 1, the rest to 0
00205 void m_multiply(const float *m1,const float *m2,float *out); // out = m1*m2 if correct index
00206 int m_inverse(const float *m,float *out);
00207 void m_transpose(const float *m,float *out);
00208 void m_transpose_rotation(const float *m,float *out);
00209 //degree
00210 void m_rotation_x(float angle,float *out);
00211 void m_rotation_y(float angle,float *out);
00212 void m_rotation_z(float angle,float *out);
00213 void m_rotation_xyz(float phi, float theta, float psi, float *out);
00214 void m_rotation_serial(float angle_1, float angle_2, float angle_3, float *out);
00215 void m_rotation_v(float angle,float *v,float *out);
00216 //radian
00217 void m_rotation_x2(float angle,float *out);
00218 void m_rotation_y2(float angle,float *out);
00219 void m_rotation_z2(float angle,float *out);
00220 void m_rotation_v2(float angle,float *v,float *out);
00221 void m_rotation_xyz2(float phi, float theta, float psi, float *out);
00222 void m_translate(const float *v,float *out);
00223 void m_scale(const float *scale,float *out);
00224 void m_look_at(const float *eye,const float *dir,const float *up,float *out);
00225 void m_shadow(const float *plane,const float *light,float *out);
00226 
00227 //added by micha
00228 float m_rotation_axis(const CMatrix3_t m,CVector3_t out);//return rotation axis and angle
00229 
00230 void q_set(const float *dir,float angle,float *out);
00231 void q_copy(const float *q,float *out);
00232 void q_complete(const float *q,float *out);
00233 void q_slerp(const float *q1,const float *q2,float t,float *out);
00234 //first q1 then q2 (out = q2 o q1 )
00235 void q_multiply(const float *q1,const float *q2,float *out);
00236 void q_to_matrix(const float *q,float *out);
00237 
00238 void q_v_mult(const float *q, const CVector3_t in, CVector3_t out);
00239 void q_clear(float *q);//{v_clear(q);q[3]=1.0f;}
00240 //void q_derivative(const float *q, float *out);
00241 
00254 float normalSample(float mean,float var);
00255 float get_angle(float c,float s);
00256 
00261 int segments_nearest_points(const CVector3_t p1,const CVector3_t vec1,
00262                             const CVector3_t p2,const CVector3_t vec2, 
00263                             float *nearest_point1, float *nearest_point2,
00264                             float *dist);
00265 
00266 // added by M.Hersch
00267 
00268 ostream& operator << (ostream& out, const CVector3_t& v);
00269 ostream& operator << (ostream& out, const CVector4_t& v);
00270 ostream& operator << (ostream& out, const CMatrix3_t& m);
00271 
00272 istream& operator>>(istream& in,CVector3_t& t);
00273 
00274 //ostream& operator << (ostream& out, const CMatrix4_t& m);
00275 
00276 
00277 
00278 inline float v_normalize(const float *v,float *out) {
00279   float length,ilength;
00280   length = (float)(sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
00281   if(length == 0) {
00282     v_clear(out);
00283     return 0;
00284   }
00285   ilength = 1.0f / length;
00286   v_scale(v,ilength,out);
00287   return length;
00288 }
00289 
00294 inline void m_rescale(float k,CMatrix4_t m, CMatrix4_t out){
00295   for(int i=0;i<16;i++){
00296     out[i] = k*m[i];
00297   }
00298 }
00299 
00300 
00301 inline void m_sub(CMatrix4_t m1,CMatrix4_t m2,CMatrix4_t out){
00302   for(int i=0;i<16;i++){
00303     out[i] = m1[i]-m2[i];
00304   }
00305 }
00306 
00307 inline void m_add(CMatrix4_t m1,CMatrix4_t m2,CMatrix4_t out){
00308   for(int i=0;i<16;i++){
00309     out[i] = m1[i]+m2[i];
00310   }
00311 }
00312 
00313 
00314 #endif /* __MATHLIB_H__ */
 All Data Structures Functions Variables

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