SO3Engine: SO3Engine/SCOLPack/SCOLMaths.cpp Source File

00001 /*
00002 -----------------------------------------------------------------------------
00003 This source file is part of OpenSpace3D
00004 For the latest info, see http://www.openspace3d.com
00005 
00006 Copyright (c) 2010 I-maginer
00007 
00008 This program is free software; you can redistribute it and/or modify it under
00009 the terms of the GNU Lesser General Public License as published by the Free Software
00010 Foundation; either version 2 of the License, or (at your option) any later
00011 version.
00012 
00013 This program is distributed in the hope that it will be useful, but WITHOUT
00014 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00015 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
00016 
00017 You should have received a copy of the GNU Lesser General Public License along with
00018 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
00019 Place - Suite 330, Boston, MA 02111-1307, USA, or go to
00020 http://www.gnu.org/copyleft/lesser.txt
00021 
00022 You may alternatively use this source under the terms of a specific version of
00023 the OpenSpace3D Unrestricted License provided you have obtained such a license from
00024 I-maginer.
00025 -----------------------------------------------------------------------------
00026 */
00027 
00028 
00036 
00037 #include "SO3SCOL.h"
00038 
00039 // Scene Graph includes
00040 #include "../SO3SceneGraph/SO3NodeScol.h"
00041 
00042 
00051 int SO3MathsQuatToEulerXYZ(mmachine m)
00052 {
00053         #ifdef SO3_DEBUG
00054         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerXYZ\n");
00055         #endif
00056 
00057         int q = MTOP(MMget(m, 0));
00058         if(q==NIL)                      
00059   {
00060     MMset(m, 0, NIL);
00061     return 0;
00062   }
00063   
00064   int x = MMfetch(m, q, 0);
00065   int y = MMfetch(m, q, 1);
00066   int z = MMfetch(m, q, 2);
00067   int w = MMfetch(m, q, 3);
00068 
00069         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00070   {
00071     MMset(m, 0, NIL);
00072     return 0;   
00073   }
00074 
00075   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00076         quat.normalise();
00077 
00078         Ogre::Matrix3 mat;
00079         Ogre::Radian xrad;
00080         Ogre::Radian yrad;
00081         Ogre::Radian zrad;
00082   
00083   quat.ToRotationMatrix(mat);
00084 
00085   mat.ToEulerAnglesXYZ(yrad, xrad, zrad);
00086 
00087         int tuple = MMmalloc(m, 3, TYPETAB);
00088         if(tuple==NIL)
00089   {
00090     MMset(m, 0, NIL);   
00091     return MERRMEM;
00092   }
00093         MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
00094         MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
00095         MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
00096   MMset(m, 0, PTOM(tuple));
00097 
00098         return 0;
00099 }
00100 
00101 
00110 int SO3MathsQuatToEulerXZY(mmachine m)
00111 {
00112         #ifdef SO3_DEBUG
00113         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerXZY\n");
00114         #endif
00115 
00116         int q = MTOP(MMget(m, 0));
00117         if(q==NIL)                      
00118   {
00119     MMset(m, 0, NIL);
00120     return 0;
00121   }
00122   
00123   int x = MMfetch(m, q, 0);
00124   int y = MMfetch(m, q, 1);
00125   int z = MMfetch(m, q, 2);
00126   int w = MMfetch(m, q, 3);
00127 
00128         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00129   {
00130     MMset(m, 0, NIL);
00131     return 0;   
00132   }
00133 
00134   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00135         quat.normalise();
00136 
00137         Ogre::Matrix3 mat;
00138         Ogre::Radian xrad;
00139         Ogre::Radian yrad;
00140         Ogre::Radian zrad;
00141 
00142         quat.ToRotationMatrix(mat);
00143   
00144         mat.ToEulerAnglesXZY(yrad, xrad, zrad);
00145 
00146         int tuple = MMmalloc(m, 3, TYPETAB);
00147         if(tuple==NIL)  
00148   {
00149     MMset(m,0,NIL);     
00150     return MERRMEM;
00151   }
00152         MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
00153         MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
00154         MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
00155   MMset(m, 0, PTOM(tuple));
00156 
00157         return 0;
00158 }
00159 
00160 
00169 int SO3MathsQuatToEulerYXZ(mmachine m)
00170 {
00171 #ifdef SO3_DEBUG
00172   MMechostr(MSKDEBUG,"SO3MathsQuatToEulerYXZ\n");
00173 #endif
00174 
00175         int q = MTOP(MMget(m, 0));
00176         if(q==NIL)                      
00177   {
00178     MMset(m, 0, NIL);
00179     return 0;
00180   }
00181   
00182   int x = MMfetch(m, q, 0);
00183   int y = MMfetch(m, q, 1);
00184   int z = MMfetch(m, q, 2);
00185   int w = MMfetch(m, q, 3);
00186 
00187         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00188   {
00189     MMset(m, 0, NIL);
00190     return 0;   
00191   }
00192 
00193   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00194   quat.normalise();
00195 
00196   Ogre::Matrix3 mat;
00197   Ogre::Radian xrad;
00198   Ogre::Radian yrad;
00199   Ogre::Radian zrad;
00200 
00201   quat.ToRotationMatrix(mat);
00202 
00203   mat.ToEulerAnglesYXZ(yrad, xrad, zrad);
00204 
00205   int tuple = MMmalloc(m, 3, TYPETAB);
00206   if(tuple==NIL)                        
00207   {
00208     MMset(m, 0, NIL);
00209     return MERRMEM;
00210   }
00211   MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
00212   MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
00213   MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
00214   MMset(m, 0, PTOM(tuple));
00215 
00216   return 0;
00217 }
00218 
00219 
00228 int SO3MathsQuatToEulerYZX(mmachine m)
00229 {
00230         #ifdef SO3_DEBUG
00231         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerYZX\n");
00232         #endif
00233 
00234         int q = MTOP(MMget(m, 0));
00235         if(q==NIL)                      
00236   {
00237     MMset(m, 0, NIL);
00238     return 0;
00239   }
00240   
00241   int x = MMfetch(m, q, 0);
00242   int y = MMfetch(m, q, 1);
00243   int z = MMfetch(m, q, 2);
00244   int w = MMfetch(m, q, 3);
00245 
00246         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00247   {
00248     MMset(m, 0, NIL);
00249     return 0;   
00250   }
00251 
00252   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00253         quat.normalise();
00254 
00255         Ogre::Matrix3 mat;
00256         Ogre::Radian xrad;
00257         Ogre::Radian yrad;
00258         Ogre::Radian zrad;
00259 
00260         quat.ToRotationMatrix(mat);
00261         mat.ToEulerAnglesYZX(yrad, xrad, zrad);
00262 
00263         int tuple = MMmalloc(m, 3, TYPETAB);
00264         if(tuple==NIL)          
00265   {
00266     MMset(m,0,NIL);     
00267     return MERRMEM;
00268   }
00269         MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
00270         MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
00271         MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
00272   MMset(m,0,PTOM(tuple));
00273         return 0;
00274 }
00275 
00276 
00285 int SO3MathsQuatToEulerZXY(mmachine m)
00286 {
00287         #ifdef SO3_DEBUG
00288         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerZXY\n");
00289         #endif
00290 
00291         int q = MTOP(MMget(m, 0));
00292         if(q==NIL)                      
00293   {
00294     MMset(m, 0, NIL);
00295     return 0;
00296   }
00297   
00298   int x = MMfetch(m, q, 0);
00299   int y = MMfetch(m, q, 1);
00300   int z = MMfetch(m, q, 2);
00301   int w = MMfetch(m, q, 3);
00302 
00303         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00304   {
00305     MMset(m, 0, NIL);
00306     return 0;   
00307   }
00308 
00309   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00310         quat.normalise();
00311 
00312         Ogre::Matrix3 mat;
00313         Ogre::Radian xrad;
00314         Ogre::Radian yrad;
00315         Ogre::Radian zrad;
00316 
00317         quat.ToRotationMatrix(mat);
00318 
00319         mat.ToEulerAnglesZXY(yrad, xrad, zrad);
00320 
00321         int tuple = MMmalloc(m, 3, TYPETAB);
00322         if(tuple==NIL)          
00323   {
00324     MMset(m, 0, NIL);   
00325     return MERRMEM;     
00326   }
00327         MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
00328         MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
00329         MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
00330   MMset(m, 0, PTOM(tuple));
00331 
00332         return 0;
00333 }
00334 
00335 
00344 int SO3MathsQuatToEulerZYX(mmachine m)
00345 {
00346         #ifdef SO3_DEBUG
00347         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerZYX\n");
00348         #endif
00349 
00350         int q = MTOP(MMget(m, 0));
00351         if(q==NIL)                      
00352   {
00353     MMset(m, 0, NIL);
00354     return 0;
00355   }
00356   
00357   int x = MMfetch(m, q, 0);
00358   int y = MMfetch(m, q, 1);
00359   int z = MMfetch(m, q, 2);
00360   int w = MMfetch(m, q, 3);
00361 
00362         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00363   {
00364     MMset(m, 0, NIL);
00365     return 0;   
00366   }
00367 
00368   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00369 
00370         quat.normalise();
00371         Ogre::Matrix3 mat;
00372         Ogre::Radian xrad;
00373         Ogre::Radian yrad;
00374         Ogre::Radian zrad;
00375 
00376         quat.ToRotationMatrix(mat);
00377 
00378         mat.ToEulerAnglesZYX(yrad, xrad, zrad);
00379 
00380         int tuple = MMmalloc(m, 3, TYPETAB);
00381         if(tuple==NIL)  
00382   {
00383     MMset(m, 0, NIL);   
00384     return MERRMEM;             
00385   }
00386         MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
00387         MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
00388         MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
00389   MMset(m, 0, PTOM(tuple));
00390 
00391         return 0;
00392 }
00393 
00394 
00403 int SO3MathsQuatToEulerDegreeXYZ(mmachine m)
00404 {
00405         #ifdef SO3_DEBUG
00406         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerDegreeXYZ\n");
00407         #endif
00408 
00409         int q = MTOP(MMget(m, 0));
00410         if(q==NIL)                      
00411   {
00412     MMset(m, 0, NIL);
00413     return 0;
00414   }
00415   
00416   int x = MMfetch(m, q, 0);
00417   int y = MMfetch(m, q, 1);
00418   int z = MMfetch(m, q, 2);
00419   int w = MMfetch(m, q, 3);
00420 
00421         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00422   {
00423     MMset(m, 0, NIL);
00424     return 0;   
00425   }
00426 
00427   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00428         quat.normalise();
00429 
00430         Ogre::Matrix3 mat;
00431         Ogre::Radian xrad;
00432         Ogre::Radian yrad;
00433         Ogre::Radian zrad;
00434 
00435         quat.ToRotationMatrix(mat);
00436 
00437         mat.ToEulerAnglesXYZ(yrad, xrad, zrad);
00438         int tuple = MMmalloc(m, 3, TYPETAB);
00439         if(tuple==NIL)  
00440   {
00441     MMset(m,0,NIL);
00442     return MERRMEM;     
00443   }
00444         MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
00445         MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
00446         MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
00447   MMset(m, 0, PTOM(tuple));
00448 
00449         return 0;
00450 }
00451 
00452 
00461 int SO3MathsQuatToEulerDegreeXZY(mmachine m)
00462 {
00463         #ifdef SO3_DEBUG
00464         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerDegreeXZY\n");
00465         #endif
00466 
00467         int q = MTOP(MMget(m, 0));
00468         if(q==NIL)                      
00469   {
00470     MMset(m, 0, NIL);
00471     return 0;
00472   }
00473   
00474   int x = MMfetch(m, q, 0);
00475   int y = MMfetch(m, q, 1);
00476   int z = MMfetch(m, q, 2);
00477   int w = MMfetch(m, q, 3);
00478 
00479         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00480   {
00481     MMset(m, 0, NIL);
00482     return 0;   
00483   }
00484 
00485   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00486         quat.normalise();
00487 
00488         Ogre::Matrix3 mat;
00489         Ogre::Radian xrad;
00490         Ogre::Radian yrad;
00491         Ogre::Radian zrad;
00492         quat.ToRotationMatrix(mat);
00493 
00494         mat.ToEulerAnglesXZY(yrad, xrad, zrad);
00495         int tuple = MMmalloc(m, 3, TYPETAB);
00496         if(tuple==NIL)          
00497   {
00498     MMset(m, 0, NIL);   
00499     return MERRMEM;     
00500   }
00501         MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
00502         MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
00503         MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
00504   MMset(m, 0, PTOM(tuple));
00505 
00506         return 0;
00507 }
00508 
00509 
00518 int SO3MathsQuatToEulerDegreeYXZ(mmachine m)
00519 {
00520         #ifdef SO3_DEBUG
00521         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerDegreeYXZ\n");
00522         #endif
00523 
00524         int q = MTOP(MMget(m, 0));
00525         if(q==NIL)                      
00526   {
00527     MMset(m, 0, NIL);
00528     return 0;
00529   }
00530   
00531   int x = MMfetch(m, q, 0);
00532   int y = MMfetch(m, q, 1);
00533   int z = MMfetch(m, q, 2);
00534   int w = MMfetch(m, q, 3);
00535 
00536         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00537   {
00538     MMset(m, 0, NIL);
00539     return 0;   
00540   }
00541 
00542   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00543         quat.normalise();
00544 
00545         Ogre::Matrix3 mat;
00546         Ogre::Radian xrad;
00547         Ogre::Radian yrad;
00548         Ogre::Radian zrad;
00549 
00550         quat.ToRotationMatrix(mat);
00551 
00552         mat.ToEulerAnglesYXZ(yrad, xrad, zrad);
00553 
00554         int tuple = MMmalloc(m, 3, TYPETAB);
00555         if(tuple==NIL)
00556   {     
00557     MMset(m, 0, NIL);   
00558     return MERRMEM;     
00559   }
00560         MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
00561         MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
00562         MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
00563   MMset(m, 0, PTOM(tuple));
00564 
00565         return 0;
00566 }
00567 
00568 
00577 int SO3MathsQuatToEulerDegreeYZX(mmachine m)
00578 {
00579         #ifdef SO3_DEBUG
00580         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerDegreeYZX\n");
00581         #endif
00582 
00583         int q = MTOP(MMget(m, 0));
00584         if(q==NIL)                      
00585   {
00586     MMset(m, 0, NIL);
00587     return 0;
00588   }
00589   
00590   int x = MMfetch(m, q, 0);
00591   int y = MMfetch(m, q, 1);
00592   int z = MMfetch(m, q, 2);
00593   int w = MMfetch(m, q, 3);
00594 
00595         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00596   {
00597     MMset(m, 0, NIL);
00598     return 0;   
00599   }
00600 
00601   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00602         quat.normalise();
00603 
00604         Ogre::Matrix3 mat;
00605         Ogre::Radian xrad;
00606         Ogre::Radian yrad;
00607         Ogre::Radian zrad;
00608 
00609         quat.ToRotationMatrix(mat);
00610 
00611         mat.ToEulerAnglesYZX(yrad, xrad, zrad);
00612 
00613         int tuple = MMmalloc(m, 3, TYPETAB);
00614         if(tuple==NIL)  
00615   {     
00616     MMset(m, 0, NIL);   
00617     return MERRMEM;             
00618   }
00619         MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
00620         MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
00621         MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
00622   MMset(m, 0, PTOM(tuple));
00623 
00624         return 0;
00625 }
00626 
00627 
00636 int SO3MathsQuatToEulerDegreeZXY(mmachine m)
00637 {
00638         #ifdef SO3_DEBUG
00639         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerDegreeZXY\n");
00640         #endif
00641 
00642         int q = MTOP(MMget(m, 0));
00643         if(q==NIL)                      
00644   {
00645     MMset(m, 0, NIL);
00646     return 0;
00647   }
00648   
00649   int x = MMfetch(m, q, 0);
00650   int y = MMfetch(m, q, 1);
00651   int z = MMfetch(m, q, 2);
00652   int w = MMfetch(m, q, 3);
00653 
00654         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00655   {
00656     MMset(m, 0, NIL);
00657     return 0;   
00658   }
00659 
00660   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00661         quat.normalise();
00662 
00663         Ogre::Matrix3 mat;
00664         Ogre::Radian xrad;
00665         Ogre::Radian yrad;
00666         Ogre::Radian zrad;
00667 
00668         quat.ToRotationMatrix(mat);
00669 
00670         mat.ToEulerAnglesZXY(yrad, xrad, zrad);
00671 
00672         int tuple = MMmalloc(m, 3, TYPETAB);
00673         if(tuple==NIL)  
00674   {
00675     MMset(m, 0, NIL);   
00676     return MERRMEM;
00677   }
00678         MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
00679         MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
00680         MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
00681   MMset(m, 0, PTOM(tuple));
00682 
00683         return 0;
00684 }
00685 
00686 
00695 int SO3MathsQuatToEulerDegreeZYX(mmachine m)
00696 {
00697         #ifdef SO3_DEBUG
00698         MMechostr(MSKDEBUG,"SO3MathsQuatToEulerDegreeZYX\n");
00699         #endif
00700 
00701         int q = MTOP(MMget(m, 0));
00702         if(q==NIL)                      
00703   {
00704     MMset(m, 0, NIL);
00705     return 0;
00706   }
00707   
00708   int x = MMfetch(m, q, 0);
00709   int y = MMfetch(m, q, 1);
00710   int z = MMfetch(m, q, 2);
00711   int w = MMfetch(m, q, 3);
00712 
00713         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
00714   {
00715     MMset(m, 0, NIL);
00716     return 0;   
00717   }
00718 
00719   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
00720         quat.normalise();
00721 
00722         Ogre::Matrix3 mat;
00723         Ogre::Radian xrad;
00724         Ogre::Radian yrad;
00725         Ogre::Radian zrad;
00726 
00727         quat.ToRotationMatrix(mat);
00728 
00729         mat.ToEulerAnglesZYX(yrad, xrad, zrad);
00730 
00731         int tuple = MMmalloc(m, 3, TYPETAB);
00732         if(tuple==NIL)  
00733   {
00734     MMset(m, 0, NIL);           
00735     return MERRMEM;
00736   }
00737         MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
00738         MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
00739         MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
00740   MMset(m, 0, PTOM(tuple));
00741 
00742         return 0;
00743 }
00744 
00745 
00754 int SO3MathsEulerXYZToQuat(mmachine m)
00755 {
00756         #ifdef SO3_DEBUG
00757         MMechostr(MSKDEBUG,"SO3MathsEulerXYZToQuat\n");
00758         #endif
00759 
00760         int vec = MTOP(MMget(m, 0));
00761         if(vec==NIL)    
00762   {
00763     MMset(m, 0, NIL);
00764     return 0;           
00765   }
00766   
00767   int x = MMfetch(m, vec, 0);
00768   int y = MMfetch(m, vec, 1);
00769   int z = MMfetch(m, vec, 2);
00770 
00771         if((x==NIL) || (y==NIL) || (z==NIL))
00772   {
00773     MMset(m, 0, NIL);
00774     return 0;   
00775   }
00776 
00777   Ogre::Radian xrad = Ogre::Radian(MTOF(x));
00778         Ogre::Radian yrad = Ogre::Radian(MTOF(y));
00779         Ogre::Radian zrad = Ogre::Radian(MTOF(z));
00780 
00781         Ogre::Matrix3 mat;
00782 
00783         mat.FromEulerAnglesXYZ(yrad, xrad, zrad);
00784         
00785   Ogre::Quaternion quat;
00786         quat.FromRotationMatrix(mat);
00787   quat.normalise();
00788 
00789         int tuple = MMmalloc(m, 4, TYPETAB);
00790         if(tuple==NIL)          
00791   {     
00792     MMset(m, 0, NIL);
00793     return MERRMEM;     
00794   }
00795         MMstore(m, tuple, 0, FTOM(quat.x));
00796         MMstore(m, tuple, 1, FTOM(quat.y));
00797         MMstore(m, tuple, 2, FTOM(quat.z));
00798         MMstore(m, tuple, 3, FTOM(quat.w));
00799   MMset(m, 0, PTOM(tuple));
00800 
00801         return 0;
00802 }
00803 
00804 
00813 int SO3MathsEulerXZYToQuat(mmachine m)
00814 {
00815         #ifdef SO3_DEBUG
00816         MMechostr(MSKDEBUG,"SO3MathsEulerXZYToQuat\n");
00817         #endif
00818 
00819         int vec = MTOP(MMget(m, 0));
00820         if(vec==NIL)    
00821   {
00822     MMset(m, 0, NIL);
00823     return 0;           
00824   }
00825   
00826   int x = MMfetch(m, vec, 0);
00827   int y = MMfetch(m, vec, 1);
00828   int z = MMfetch(m, vec, 2);
00829 
00830         if((x==NIL) || (y==NIL) || (z==NIL))
00831   {
00832     MMset(m, 0, NIL);
00833     return 0;   
00834   }
00835 
00836   Ogre::Radian xrad = Ogre::Radian(MTOF(x));
00837         Ogre::Radian yrad = Ogre::Radian(MTOF(y));
00838         Ogre::Radian zrad = Ogre::Radian(MTOF(z));
00839 
00840         Ogre::Matrix3 mat;
00841 
00842         mat.FromEulerAnglesXZY(yrad, xrad, zrad);
00843 
00844         Ogre::Quaternion quat;
00845         quat.FromRotationMatrix(mat);
00846   quat.normalise();
00847 
00848         int tuple = MMmalloc(m, 4, TYPETAB);
00849         if(tuple==NIL)                          
00850   {
00851     MMset(m, 0, NIL);   
00852     return MERRMEM;
00853   }
00854         MMstore(m, tuple, 0, FTOM(quat.x));
00855         MMstore(m, tuple, 1, FTOM(quat.y));
00856         MMstore(m, tuple, 2, FTOM(quat.z));
00857         MMstore(m, tuple, 3, FTOM(quat.w));
00858   MMset(m, 0, PTOM(tuple));
00859 
00860         return 0;
00861 }
00862 
00863 
00872 int SO3MathsEulerYXZToQuat(mmachine m)
00873 {
00874         #ifdef SO3_DEBUG
00875         MMechostr(MSKDEBUG,"SO3MathsEulerYXZToQuat\n");
00876         #endif
00877 
00878         int vec = MTOP(MMget(m, 0));
00879         if(vec==NIL)    
00880   {
00881     MMset(m, 0, NIL);
00882     return 0;           
00883   }
00884   
00885   int x = MMfetch(m, vec, 0);
00886   int y = MMfetch(m, vec, 1);
00887   int z = MMfetch(m, vec, 2);
00888 
00889         if((x==NIL) || (y==NIL) || (z==NIL))
00890   {
00891     MMset(m, 0, NIL);
00892     return 0;   
00893   }
00894 
00895   Ogre::Radian xrad = Ogre::Radian(MTOF(x));
00896         Ogre::Radian yrad = Ogre::Radian(MTOF(y));
00897         Ogre::Radian zrad = Ogre::Radian(MTOF(z));
00898 
00899         Ogre::Matrix3 mat;
00900 
00901         mat.FromEulerAnglesYXZ(yrad, xrad, zrad);
00902 
00903         Ogre::Quaternion quat;
00904         quat.FromRotationMatrix(mat);
00905   quat.normalise();
00906 
00907         int tuple = MMmalloc(m, 4, TYPETAB);
00908         if(tuple==NIL)          
00909   {
00910     MMset(m, 0, NIL);   
00911     return MERRMEM;
00912   }
00913         MMstore(m, tuple, 0, FTOM(quat.x));
00914         MMstore(m, tuple, 1, FTOM(quat.y));
00915         MMstore(m, tuple, 2, FTOM(quat.z));
00916         MMstore(m, tuple, 3, FTOM(quat.w));
00917   MMset(m, 0, PTOM(tuple));
00918 
00919         return 0;
00920 }
00921 
00922 
00931 int SO3MathsEulerYZXToQuat(mmachine m)
00932 {
00933         #ifdef SO3_DEBUG
00934         MMechostr(MSKDEBUG,"SO3MathsEulerYZXToQuat\n");
00935         #endif
00936 
00937         int vec = MTOP(MMget(m, 0));
00938         if(vec==NIL)    
00939   {
00940     MMset(m, 0, NIL);
00941     return 0;           
00942   }
00943   
00944   int x = MMfetch(m, vec, 0);
00945   int y = MMfetch(m, vec, 1);
00946   int z = MMfetch(m, vec, 2);
00947 
00948         if((x==NIL) || (y==NIL) || (z==NIL))
00949   {
00950     MMset(m, 0, NIL);
00951     return 0;   
00952   }
00953 
00954   Ogre::Radian xrad = Ogre::Radian(MTOF(x));
00955         Ogre::Radian yrad = Ogre::Radian(MTOF(y));
00956         Ogre::Radian zrad = Ogre::Radian(MTOF(z));
00957 
00958   Ogre::Matrix3 mat;
00959 
00960         mat.FromEulerAnglesYZX(yrad, xrad, zrad);
00961 
00962         Ogre::Quaternion quat;
00963         quat.FromRotationMatrix(mat);
00964   quat.normalise();
00965 
00966         int tuple = MMmalloc(m, 4, TYPETAB);
00967         if(tuple==NIL)                  
00968   {
00969     MMset(m, 0, NIL);   
00970     return MERRMEM;             
00971   }
00972         MMstore(m, tuple, 0, FTOM(quat.x));
00973         MMstore(m, tuple, 1, FTOM(quat.y));
00974         MMstore(m, tuple, 2, FTOM(quat.z));
00975         MMstore(m, tuple, 3, FTOM(quat.w));
00976   MMset(m, 0, PTOM(tuple));
00977 
00978         return 0;
00979 }
00980 
00981 
00990 int SO3MathsEulerZYXToQuat(mmachine m)
00991 {
00992         #ifdef SO3_DEBUG
00993         MMechostr(MSKDEBUG,"SO3MathsEulerZYXToQuat\n");
00994         #endif
00995 
00996         int vec = MTOP(MMget(m, 0));
00997         if(vec==NIL)    
00998   {
00999     MMset(m, 0, NIL);
01000     return 0;           
01001   }
01002   
01003   int x = MMfetch(m, vec, 0);
01004   int y = MMfetch(m, vec, 1);
01005   int z = MMfetch(m, vec, 2);
01006 
01007         if((x==NIL) || (y==NIL) || (z==NIL))
01008   {
01009     MMset(m, 0, NIL);
01010     return 0;   
01011   }
01012 
01013   Ogre::Radian xrad = Ogre::Radian(MTOF(x));
01014         Ogre::Radian yrad = Ogre::Radian(MTOF(y));
01015         Ogre::Radian zrad = Ogre::Radian(MTOF(z));
01016 
01017         Ogre::Matrix3 mat;
01018 
01019         mat.FromEulerAnglesZYX(yrad, xrad, zrad);
01020 
01021         Ogre::Quaternion quat;
01022         quat.FromRotationMatrix(mat);
01023   quat.normalise();
01024 
01025         int tuple = MMmalloc(m, 4, TYPETAB);
01026         if(tuple==NIL)  
01027   {     
01028     MMset(m, 0, NIL);           
01029     return MERRMEM;     
01030   }
01031         MMstore(m, tuple, 0, FTOM(quat.x));
01032         MMstore(m, tuple, 1, FTOM(quat.y));
01033         MMstore(m, tuple, 2, FTOM(quat.z));
01034         MMstore(m, tuple, 3, FTOM(quat.w));
01035   MMset(m, 0, PTOM(tuple));
01036 
01037         return 0;
01038 }
01039 
01040 
01049 int SO3MathsEulerZXYToQuat(mmachine m)
01050 {
01051         #ifdef SO3_DEBUG
01052         MMechostr(MSKDEBUG,"SO3MathsEulerZXYToQuat\n");
01053         #endif
01054 
01055         int vec = MTOP(MMget(m, 0));
01056         if(vec==NIL)    
01057   {
01058     MMset(m, 0, NIL);
01059     return 0;           
01060   }
01061   
01062   int x = MMfetch(m, vec, 0);
01063   int y = MMfetch(m, vec, 1);
01064   int z = MMfetch(m, vec, 2);
01065 
01066         if((x==NIL) || (y==NIL) || (z==NIL))
01067   {
01068     MMset(m, 0, NIL);
01069     return 0;   
01070   }
01071 
01072   Ogre::Radian xrad = Ogre::Radian(MTOF(x));
01073         Ogre::Radian yrad = Ogre::Radian(MTOF(y));
01074         Ogre::Radian zrad = Ogre::Radian(MTOF(z));
01075 
01076         Ogre::Matrix3 mat;
01077 
01078         mat.FromEulerAnglesZXY(yrad, xrad, zrad);
01079 
01080         Ogre::Quaternion quat;
01081         quat.FromRotationMatrix(mat);
01082   quat.normalise();
01083 
01084         int tuple = MMmalloc(m, 4, TYPETAB);
01085         if(tuple==NIL)  
01086   {
01087     MMset(m, 0, NIL);           
01088     return MERRMEM;
01089   }
01090         MMstore(m, tuple, 0, FTOM(quat.x));
01091         MMstore(m, tuple, 1, FTOM(quat.y));
01092         MMstore(m, tuple, 2, FTOM(quat.z));
01093         MMstore(m, tuple, 3, FTOM(quat.w));
01094   MMset(m, 0, PTOM(tuple));
01095 
01096         return 0;
01097 }
01098 
01099 
01109 int SO3MathsQuatDiff(mmachine m)
01110 {
01111         #ifdef SO3_DEBUG
01112         MMechostr(MSKDEBUG,"SO3MathsQuatDiff\n");
01113         #endif
01114 
01115         int q2 = MTOP(MMpull(m));
01116         int q1 = MTOP(MMget(m, 0));
01117         
01118         if(q1==NIL || q2==NIL)  
01119   {
01120     MMset(m, 0, NIL);   
01121     return 0;   
01122   }
01123 
01124         int x1 = MMfetch(m, q1, 0);
01125         int y1 = MMfetch(m, q1, 1);
01126         int z1 = MMfetch(m, q1, 2);
01127         int w1 = MMfetch(m, q1, 3);
01128 
01129         int x2 = MMfetch(m, q2, 0);
01130         int y2 = MMfetch(m, q2, 1);
01131         int z2 = MMfetch(m, q2, 2);
01132         int w2 = MMfetch(m, q2, 3);
01133 
01134         if((x1==NIL) || (y1==NIL) || (z1==NIL) || (w1==NIL) || (x2==NIL) || (y2==NIL) || (z2==NIL) || (w2==NIL))
01135   {
01136     MMset(m, 0, NIL);   
01137     return 0;   
01138   }
01139         
01140   Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
01141   Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
01142 
01143         Ogre::Quaternion quatResult = quat1 * quat2.Inverse();
01144   quatResult.normalise();
01145 
01146         int tuple = MMmalloc(m, 4, TYPETAB);
01147         if(tuple==NIL)          
01148   {
01149     MMset(m, 0, NIL);           
01150     return MERRMEM;
01151   }
01152         MMstore(m, tuple, 0, FTOM(quatResult.x));
01153         MMstore(m, tuple, 1, FTOM(quatResult.y));
01154         MMstore(m, tuple, 2, FTOM(quatResult.z));
01155         MMstore(m, tuple, 3, FTOM(quatResult.w));
01156   MMset(m, 0, PTOM(tuple));
01157 
01158         return 0;
01159 }
01160 
01161 
01171 int SO3MathsQuatSubstract(mmachine m)
01172 {
01173         #ifdef SO3_DEBUG
01174         MMechostr(MSKDEBUG,"SO3MathsQuatSubstract\n");
01175         #endif
01176 
01177         int q2 = MTOP(MMpull(m));
01178         int q1 = MTOP(MMget(m, 0));
01179         
01180         if(q1==NIL || q2==NIL)  
01181   {
01182     MMset(m, 0, NIL);   
01183     return 0;
01184   }
01185 
01186         int x1 = MMfetch(m, q1, 0);
01187         int y1 = MMfetch(m, q1, 1);
01188         int z1 = MMfetch(m, q1, 2);
01189         int w1 = MMfetch(m, q1, 3);
01190 
01191         int x2 = MMfetch(m, q2, 0);
01192         int y2 = MMfetch(m, q2, 1);
01193         int z2 = MMfetch(m, q2, 2);
01194         int w2 = MMfetch(m, q2, 3);
01195 
01196         if((x1==NIL) || (y1==NIL) || (z1==NIL) || (w1==NIL) || (x2==NIL) || (y2==NIL) || (z2==NIL) || (w2==NIL))
01197   {
01198     MMset(m, 0, NIL);   
01199     return 0;   
01200   }
01201         
01202   Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
01203 
01204   Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
01205 
01206         Ogre::Quaternion quatResult = quat1.Inverse() * quat2;
01207   quatResult.normalise();
01208 
01209         int tuple = MMmalloc(m, 4, TYPETAB);
01210         if(tuple==NIL)  
01211   {
01212     MMset(m, 0, NIL);   
01213     return MERRMEM;     
01214   }
01215         MMstore(m, tuple, 0, FTOM(quatResult.x));
01216         MMstore(m, tuple, 1, FTOM(quatResult.y));
01217         MMstore(m, tuple, 2, FTOM(quatResult.z));
01218         MMstore(m, tuple, 3, FTOM(quatResult.w));
01219   MMset(m, 0, PTOM(tuple));
01220 
01221         return 0;
01222 }
01223 
01224 
01234 int SO3MathsQuatAdd(mmachine m)
01235 {
01236         #ifdef SO3_DEBUG
01237         MMechostr(MSKDEBUG,"SO3MathsQuatAdd\n");
01238         #endif
01239 
01240         int q2 = MTOP(MMpull(m));
01241         int q1 = MTOP(MMget(m, 0));
01242 
01243         if(q1==NIL || q2==NIL)  
01244   {
01245     MMset(m, 0, NIL);   
01246     return 0;   
01247   }
01248 
01249         int x1 = MMfetch(m, q1, 0);
01250         int y1 = MMfetch(m, q1, 1);
01251         int z1 = MMfetch(m, q1, 2);
01252         int w1 = MMfetch(m, q1, 3);
01253 
01254         int x2 = MMfetch(m, q2, 0);
01255         int y2 = MMfetch(m, q2, 1);
01256         int z2 = MMfetch(m, q2, 2);
01257         int w2 = MMfetch(m, q2, 3);
01258 
01259         if((x1==NIL) || (y1==NIL) || (z1==NIL) || (w1==NIL) || (x2==NIL) || (y2==NIL) || (z2==NIL) || (w2==NIL))
01260   {
01261     MMset(m, 0, NIL);   
01262     return 0;   
01263   }
01264         
01265   Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
01266   quat1.normalise();
01267   Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
01268   quat2.normalise();
01269 
01270         Ogre::Quaternion quatResult = quat1 * quat2;
01271   quatResult.normalise();
01272 
01273         int tuple = MMmalloc(m, 4, TYPETAB);
01274         if(tuple==NIL)  
01275   {
01276     MMset(m, 0, NIL);   
01277     return MERRMEM;     
01278   }
01279 
01280         MMstore(m, tuple, 0, FTOM(quatResult.x));
01281         MMstore(m, tuple, 1, FTOM(quatResult.y));
01282         MMstore(m, tuple, 2, FTOM(quatResult.z));
01283         MMstore(m, tuple, 3, FTOM(quatResult.w));
01284   MMset(m, 0, PTOM(tuple));
01285 
01286         return 0;
01287 }
01288 
01289 
01301 int SO3MathsQuatInterpolate(mmachine m)
01302 {
01303         #ifdef SO3_DEBUG
01304         MMechostr(MSKDEBUG,"SO3MathsQuatInterpolate\n");
01305         #endif
01306 
01307         int ishort = MTOI(MMpull(m));
01308         int coef = MMpull(m);
01309         int q2 = MTOP(MMpull(m));
01310         int q1 = MTOP(MMget(m, 0));
01311         
01312         if((q1 == NIL) || (q2 == NIL) || (coef == NIL)) 
01313   {     
01314     MMset(m, 0, NIL);
01315     return 0;   
01316   }
01317 
01318         bool bshort = false;
01319         if(ishort == 1)
01320                 bshort = true;
01321  
01322         int x1 = MMfetch(m, q1, 0);
01323         int y1 = MMfetch(m, q1, 1);
01324         int z1 = MMfetch(m, q1, 2);
01325         int w1 = MMfetch(m, q1, 3);
01326 
01327         int x2 = MMfetch(m, q2, 0);
01328         int y2 = MMfetch(m, q2, 1);
01329         int z2 = MMfetch(m, q2, 2);
01330         int w2 = MMfetch(m, q2, 3);
01331 
01332         if((x1==NIL) || (y1==NIL) || (z1==NIL) || (w1==NIL) || (x2==NIL) || (y2==NIL) || (z2==NIL) || (w2==NIL))
01333   {
01334     MMset(m, 0, NIL);   
01335     return 0;   
01336   }
01337         
01338   Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
01339   quat1.normalise();
01340   Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
01341   quat2.normalise();
01342 
01343   Ogre::Quaternion quatResult = Ogre::Quaternion::Slerp((Ogre::Real)MTOF(coef), quat1, quat2, bshort);
01344   quatResult.normalise();
01345 
01346         int tuple = MMmalloc(m, 4, TYPETAB);
01347         if(tuple==NIL)
01348   {     
01349     MMset(m, 0, NIL);
01350     return MERRMEM;     
01351   }
01352 
01353         MMstore(m, tuple, 0, FTOM(quatResult.x));
01354         MMstore(m, tuple, 1, FTOM(quatResult.y));
01355         MMstore(m, tuple, 2, FTOM(quatResult.z));
01356         MMstore(m, tuple, 3, FTOM(quatResult.w));
01357   MMset(m, 0, PTOM(tuple));
01358 
01359         return 0;
01360 }
01361 
01362 
01371 int SO3MathsDegreeToRadian(mmachine m)
01372 {
01373         #ifdef SO3_DEBUG
01374         MMechostr(MSKDEBUG,"SO3MathsDegreeToRadian\n");
01375         #endif
01376 
01377         int val = MMget(m, 0);
01378         if(val==NIL)                    
01379   {     
01380     MMset(m, 0, NIL);   
01381     return 0;           
01382   }
01383 
01384         Ogre::Degree deg = Ogre::Degree(MTOF(val));
01385         float rad = deg.valueRadians();
01386 
01387   MMset(m, 0, FTOM(rad));
01388         
01389   return 0;
01390 }
01391 
01392 
01401 int SO3MathsRadianToDegree(mmachine m)
01402 {
01403         #ifdef SO3_DEBUG
01404         MMechostr(MSKDEBUG,"SO3MathsRadianToDegree\n");
01405         #endif
01406 
01407         int val = MMget(m, 0);
01408         if(val==NIL)            
01409   {
01410     MMset(m, 0, NIL);
01411     return 0;           
01412   }
01413 
01414         Ogre::Radian rad = Ogre::Radian(MTOF(val));
01415         float deg = rad.valueDegrees();
01416 
01417   MMset(m, 0, FTOM(deg));
01418 
01419         return 0;
01420 }
01421 
01422 
01432 int SO3MathsQuatGetRoll(mmachine m)
01433 {
01434         #ifdef SO3_DEBUG
01435         MMechostr(MSKDEBUG,"SO3MathsQuatGetRoll\n");
01436         #endif
01437 
01438         int booleen = MTOI(MMpull(m));
01439         int q = MMget(m, 0);
01440         if(q==NIL)                      
01441   {
01442     MMset(m, 0, NIL);
01443     return 0;
01444   }
01445   
01446   int x = MMfetch(m, MTOP(q), 0);
01447   int y = MMfetch(m, MTOP(q), 1);
01448   int z = MMfetch(m, MTOP(q), 2);
01449   int w = MMfetch(m, MTOP(q), 3);
01450 
01451         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
01452   {
01453     MMset(m, 0, NIL);
01454     return 0;   
01455   }
01456 
01457   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
01458         quat.normalise();
01459 
01460         Ogre::Radian roll;
01461 
01462         if(booleen == 0) 
01463     roll = quat.getRoll(false);
01464         else 
01465     roll = quat.getRoll(true);
01466 
01467   MMset(m, 0, FTOM(roll.valueRadians()));
01468         return 0;
01469 }
01470 
01471 
01481 int SO3MathsQuatGetYaw(mmachine m)
01482 {
01483         #ifdef SO3_DEBUG
01484         MMechostr(MSKDEBUG,"SO3MathsQuatGetYaw\n");
01485         #endif
01486 
01487         int booleen = MTOI(MMpull(m));
01488         int q = MMget(m, 0);
01489         if(q==NIL)                      
01490   {
01491     MMset(m, 0, NIL);
01492     return 0;
01493   }
01494   
01495   int x = MMfetch(m, MTOP(q), 0);
01496   int y = MMfetch(m, MTOP(q), 1);
01497   int z = MMfetch(m, MTOP(q), 2);
01498   int w = MMfetch(m, MTOP(q), 3);
01499 
01500         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
01501   {
01502     MMset(m, 0, NIL);
01503     return 0;   
01504   }
01505 
01506   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
01507         quat.normalise();
01508 
01509         Ogre::Radian yaw;
01510         if(booleen == 0) 
01511     yaw = quat.getYaw(false);
01512         else
01513     yaw = quat.getYaw(true);
01514 
01515         MMset(m, 0, FTOM(yaw.valueRadians()));
01516         return 0;
01517 }
01518 
01519 
01529 int SO3MathsQuatGetPitch(mmachine m)
01530 {
01531         #ifdef SO3_DEBUG
01532         MMechostr(MSKDEBUG,"SO3MathsQuatGetPitch\n");
01533         #endif
01534 
01535         int booleen = MTOI(MMpull(m));
01536         int q = MMget(m, 0);
01537         if(q==NIL)                      
01538   {
01539     MMset(m, 0, NIL);
01540     return 0;
01541   }
01542   
01543   int x = MMfetch(m, MTOP(q), 0);
01544   int y = MMfetch(m, MTOP(q), 1);
01545   int z = MMfetch(m, MTOP(q), 2);
01546   int w = MMfetch(m, MTOP(q), 3);
01547 
01548         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
01549   {
01550     MMset(m, 0, NIL);
01551     return 0;   
01552   }
01553 
01554   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
01555         quat.normalise();
01556 
01557         Ogre::Radian pitch;
01558         if(booleen == 0) 
01559     pitch = quat.getPitch(false);
01560         else 
01561     pitch = quat.getPitch(true);
01562 
01563   MMset(m, 0, FTOM(pitch.valueRadians()));
01564         return 0;
01565 }
01566 
01567 
01577 int SO3MathsQuatGetDegreeRoll(mmachine m)
01578 {
01579         #ifdef SO3_DEBUG
01580         MMechostr(MSKDEBUG,"SO3MathsQuatGetRoll\n");
01581         #endif
01582 
01583         int booleen = MTOI(MMpull(m));
01584         int q = MMget(m, 0);
01585         if(q==NIL)
01586   {
01587     MMset(m, 0, NIL);
01588     return 0;
01589   }
01590   
01591   int x = MMfetch(m, MTOP(q), 0);
01592   int y = MMfetch(m, MTOP(q), 1);
01593   int z = MMfetch(m, MTOP(q), 2);
01594   int w = MMfetch(m, MTOP(q), 3);
01595 
01596         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
01597   {
01598     MMset(m, 0, NIL);
01599     return 0;   
01600   }
01601 
01602   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
01603   quat.normalise();
01604 
01605         Ogre::Radian roll;
01606         if(booleen == 0) 
01607     roll = quat.getRoll(false);
01608         else
01609     roll = quat.getRoll(true);
01610 
01611         MMset(m, 0, FTOM(roll.valueDegrees()));
01612         return 0;
01613 }
01614 
01615 
01625 int SO3MathsQuatGetDegreeYaw(mmachine m)
01626 {
01627         #ifdef SO3_DEBUG
01628         MMechostr(MSKDEBUG,"SO3MathsQuatGetYaw\n");
01629         #endif
01630 
01631         int booleen = MTOI(MMpull(m));
01632         int q = MMget(m, 0);
01633         if(q==NIL)                      
01634   {
01635     MMset(m, 0, NIL);
01636     return 0;
01637   }
01638   
01639   int x = MMfetch(m, MTOP(q), 0);
01640   int y = MMfetch(m, MTOP(q), 1);
01641   int z = MMfetch(m, MTOP(q), 2);
01642   int w = MMfetch(m, MTOP(q), 3);
01643 
01644         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
01645   {
01646     MMset(m, 0, NIL);
01647     return 0;   
01648   }
01649 
01650   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
01651   quat.normalise();
01652 
01653         Ogre::Radian yaw;
01654         if(booleen == 0) 
01655     yaw = quat.getYaw(false);
01656         else 
01657     yaw = quat.getYaw(true);
01658 
01659         MMset(m, 0, FTOM(yaw.valueDegrees()));
01660         return 0;
01661 }
01662 
01663 
01673 int SO3MathsQuatGetDegreePitch(mmachine m)
01674 {
01675         #ifdef SO3_DEBUG
01676         MMechostr(MSKDEBUG,"SO3MathsQuatGetPitch\n");
01677         #endif
01678 
01679         int booleen = MTOI(MMpull(m));
01680         int q = MMget(m, 0);
01681         if(q==NIL)                      
01682   {
01683     MMset(m, 0, NIL);
01684     return 0;
01685   }
01686   
01687   int x = MMfetch(m, MTOP(q), 0);
01688   int y = MMfetch(m, MTOP(q), 1);
01689   int z = MMfetch(m, MTOP(q), 2);
01690   int w = MMfetch(m, MTOP(q), 3);
01691 
01692         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
01693   {
01694     MMset(m, 0, NIL);
01695     return 0;   
01696   }
01697 
01698   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
01699   quat.normalise();
01700 
01701         Ogre::Radian pitch;
01702         if(booleen == 0)
01703     pitch = quat.getPitch(false);
01704         else
01705     pitch = quat.getPitch(true);
01706 
01707   MMset(m, 0, FTOM(pitch.valueDegrees()));
01708         return 0;
01709 }
01710 
01711 
01720 int SO3MathsQuatToAxes(mmachine m)
01721 {
01722         #ifdef SO3_DEBUG
01723         MMechostr(MSKDEBUG,"SO3MathsQuatToAxes\n");
01724         #endif
01725 
01726         int q = MMget(m, 0);
01727         if(q==NIL)                      
01728   {
01729     MMset(m, 0, NIL);
01730     return 0;
01731   }
01732   
01733   int x = MMfetch(m, MTOP(q), 0);
01734   int y = MMfetch(m, MTOP(q), 1);
01735   int z = MMfetch(m, MTOP(q), 2);
01736   int w = MMfetch(m, MTOP(q), 3);
01737 
01738         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
01739   {
01740     MMset(m, 0, NIL);
01741     return 0;   
01742   }
01743 
01744   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
01745   quat.normalise();
01746 
01747         Ogre::Vector3 xAxis;
01748         Ogre::Vector3 yAxis;
01749         Ogre::Vector3 zAxis;
01750 
01751         quat.ToAxes(xAxis, yAxis, zAxis);
01752 
01753         // x
01754         int xtuple = MMmalloc(m, 3, TYPETAB);
01755         if(x==NIL)                      
01756   {
01757     MMset(m, 0, NIL);   
01758     return MERRMEM;
01759   }
01760         MMstore(m, xtuple, 0, FTOM(xAxis.x));
01761         MMstore(m, xtuple, 1, FTOM(xAxis.y));
01762         MMstore(m, xtuple, 2, FTOM(xAxis.z));
01763         MMpush(m, PTOM(xtuple));
01764 
01765         // y
01766         int ytuple = MMmalloc(m, 3, TYPETAB);
01767         if(y==NIL)                                      
01768   {
01769     MMset(m, 0, NIL);
01770     return MERRMEM;
01771   }
01772         MMstore(m, ytuple, 0, FTOM(yAxis.x));
01773         MMstore(m, ytuple, 1, FTOM(yAxis.y));
01774         MMstore(m, ytuple, 2, FTOM(yAxis.z));
01775         MMpush(m, PTOM(ytuple));
01776 
01777         // z
01778         int ztuple = MMmalloc(m, 3, TYPETAB);
01779         if(z==NIL)      
01780   {     
01781     MMset(m, 0, NIL);
01782     return MERRMEM;     
01783   }
01784 
01785         MMstore(m, ztuple, 0, FTOM(zAxis.x));
01786         MMstore(m, ztuple, 1, FTOM(zAxis.y));
01787         MMstore(m, ztuple, 2, FTOM(zAxis.z));
01788         MMpush(m, PTOM(ztuple));
01789 
01790   // FINAL TUPPLE 
01791   int result = MMmalloc(m, 3, TYPETAB);
01792         if(result==NIL)         
01793   {
01794     MMset(m, 0, NIL);
01795     return MERRMEM;
01796   }
01797         MMstore(m, result, 2, MMpull(m) );
01798         MMstore(m, result, 1, MMpull(m) );
01799         MMstore(m, result, 0, MMpull(m) );
01800         MMset(m, 0, PTOM(result));
01801 
01802         return 0;
01803 }
01804 
01813 int SO3MathsQuatFromAngle(mmachine m)
01814 {
01815         #ifdef SO3_DEBUG
01816         MMechostr(MSKDEBUG,"SO3MathsQuatFromAngle\n");
01817         #endif
01818 
01819         int vec = MMget(m, 0);
01820         if(vec==NIL)    
01821   {
01822     MMset(m, 0, NIL);
01823     return 0;
01824   }
01825 
01826   int x = MMfetch(m, MTOP(vec), 0);
01827   int y = MMfetch(m, MTOP(vec), 1);
01828   int z = MMfetch(m, MTOP(vec), 2);
01829 
01830         if((x==NIL) || (y==NIL) || (z==NIL))
01831   {
01832     MMset(m, 0, NIL);
01833     return 0;   
01834   }
01835 
01836         Ogre::Quaternion quatX;
01837   Ogre::Quaternion quatY;
01838   Ogre::Quaternion quatZ;
01839 
01840   quatX.FromAngleAxis(Ogre::Radian(MTOF(x)), Ogre::Vector3(1,0,0));
01841   quatY.FromAngleAxis(Ogre::Radian(MTOF(y)), Ogre::Vector3(0,1,0));
01842   quatZ.FromAngleAxis(Ogre::Radian(MTOF(z)), Ogre::Vector3(0,0,1));
01843 
01844   Ogre::Quaternion quatResult;
01845   quatResult = quatX * quatY * quatZ;
01846   quatResult.normalise();
01847   
01848         int tuple = MMmalloc(m, 4, TYPETAB);
01849         if(tuple==NIL)
01850   {
01851     MMset(m, 0, NIL);
01852     return MERRMEM;
01853   }
01854         MMstore(m, tuple, 0, FTOM((quatResult.x)));
01855         MMstore(m, tuple, 1, FTOM((quatResult.y)));
01856         MMstore(m, tuple, 2, FTOM((quatResult.z)));
01857         MMstore(m, tuple, 3, FTOM((quatResult.w)));
01858         MMset(m, 0, PTOM(tuple));
01859 
01860         return 0;
01861 }
01862 
01871 int SO3MathsQuatFromDegreeAngle(mmachine m)
01872 {
01873         #ifdef SO3_DEBUG
01874         MMechostr(MSKDEBUG,"SO3MathsQuatFromDegreeAngle\n");
01875         #endif
01876 
01877         int vec = MMget(m, 0);
01878         if(vec==NIL)    
01879   {
01880     MMset(m, 0, NIL);
01881     return 0;
01882   }
01883 
01884   int x = MMfetch(m, MTOP(vec), 0);
01885   int y = MMfetch(m, MTOP(vec), 1);
01886   int z = MMfetch(m, MTOP(vec), 2);
01887 
01888         if((x==NIL) || (y==NIL) || (z==NIL))
01889   {
01890     MMset(m, 0, NIL);
01891     return 0;   
01892   }
01893 
01894         Ogre::Quaternion quatX;
01895   Ogre::Quaternion quatY;
01896   Ogre::Quaternion quatZ;
01897   
01898   Ogre::Degree dx = (Ogre::Degree)MTOF(x);
01899   Ogre::Degree dy = (Ogre::Degree)MTOF(y);
01900   Ogre::Degree dz = (Ogre::Degree)MTOF(z);
01901   quatX.FromAngleAxis((Ogre::Radian)dx.valueRadians(), Ogre::Vector3(1,0,0));
01902   quatY.FromAngleAxis((Ogre::Radian)dy.valueRadians(), Ogre::Vector3(0,1,0));
01903   quatZ.FromAngleAxis((Ogre::Radian)dz.valueRadians(), Ogre::Vector3(0,0,1));
01904 
01905   Ogre::Quaternion quatResult;
01906   quatResult = quatX * quatY * quatZ;
01907   quatResult.normalise();
01908   
01909         int tuple = MMmalloc(m, 4, TYPETAB);
01910         if(tuple==NIL)
01911   {
01912     MMset(m, 0, NIL);
01913     return MERRMEM;
01914   }
01915         MMstore(m, tuple, 0, FTOM((quatResult.x)));
01916         MMstore(m, tuple, 1, FTOM((quatResult.y)));
01917         MMstore(m, tuple, 2, FTOM((quatResult.z)));
01918         MMstore(m, tuple, 3, FTOM((quatResult.w)));
01919         MMset(m, 0, PTOM(tuple));
01920 
01921         return 0;
01922 }
01923 
01924 
01934 int SO3MathsQuatFromAngleAxis(mmachine m)
01935 {
01936         #ifdef SO3_DEBUG
01937         MMechostr(MSKDEBUG,"SO3MathsQuatFromAngleAxis\n");
01938         #endif
01939   
01940   int rad = MMpull(m);
01941         int vec = MMget(m, 0);
01942         if((vec==NIL)    || (rad == NIL))
01943   {
01944     MMset(m, 0, NIL);
01945     return 0;
01946   }
01947 
01948   int x = MMfetch(m, MTOP(vec), 0);
01949   int y = MMfetch(m, MTOP(vec), 1);
01950   int z = MMfetch(m, MTOP(vec), 2);
01951 
01952         if((x==NIL) || (y==NIL) || (z==NIL))
01953   {
01954     MMset(m, 0, NIL);
01955     return 0;   
01956   }
01957 
01958   Ogre::Quaternion quatResult;
01959   Ogre::Vector3 axis(MTOF(x), MTOF(y), MTOF(z));
01960   quatResult.FromAngleAxis(Ogre::Radian(MTOF(rad)), axis);
01961   quatResult.normalise();
01962   
01963         int tuple = MMmalloc(m, 4, TYPETAB);
01964         if(tuple==NIL)
01965   {
01966     MMset(m, 0, NIL);
01967     return MERRMEM;
01968   }
01969         MMstore(m, tuple, 0, FTOM((quatResult.x)));
01970         MMstore(m, tuple, 1, FTOM((quatResult.y)));
01971         MMstore(m, tuple, 2, FTOM((quatResult.z)));
01972         MMstore(m, tuple, 3, FTOM((quatResult.w)));
01973         MMset(m, 0, PTOM(tuple));
01974 
01975         return 0;
01976 }
01977 
01986 int SO3MathsQuatFromAxes(mmachine m)
01987 {
01988         #ifdef SO3_DEBUG
01989         MMechostr(MSKDEBUG,"SO3MathsQuatFromAxes\n");
01990         #endif
01991 
01992         int x = MTOP(MMpull(m));
01993         int y = MTOP(MMpull(m));
01994         int z = MTOP(MMget(m,0));
01995         if(x==NIL || y==NIL || z==NIL)  
01996   {
01997     MMset(m, 0, NIL);
01998     return 0;
01999   }
02000 
02001         Ogre::Vector3 xAxis;
02002         xAxis.x = MTOF(MMfetch(m,x,0));
02003         xAxis.y = MTOF(MMfetch(m,x,1));
02004         xAxis.z = MTOF(MMfetch(m,x,2));
02005 
02006         Ogre::Vector3 yAxis;
02007         yAxis.x = MTOF(MMfetch(m,y,0));
02008         yAxis.y = MTOF(MMfetch(m,y,1));
02009         yAxis.z = MTOF(MMfetch(m,y,2));
02010 
02011         Ogre::Vector3 zAxis;
02012         zAxis.x = MTOF(MMfetch(m,z,0));
02013         zAxis.y = MTOF(MMfetch(m,z,1));
02014         zAxis.z = MTOF(MMfetch(m,z,2));
02015 
02016 
02017         Ogre::Quaternion quat;
02018 
02019         quat.FromAxes(xAxis,yAxis,zAxis);
02020   quat.normalise();
02021 
02022         int tuple = MMmalloc(m, 4, TYPETAB);
02023         if(tuple==NIL)
02024   {
02025     MMset(m, 0, NIL);
02026     return MERRMEM;
02027   }
02028         MMstore(m, tuple, 0, FTOM((quat.x)));
02029         MMstore(m, tuple, 1, FTOM((quat.y)));
02030         MMstore(m, tuple, 2, FTOM((quat.z)));
02031         MMstore(m, tuple, 3, FTOM((quat.w)));
02032         MMset(m, 0, PTOM(tuple));
02033 
02034         return 0;
02035 }
02036 
02037 
02046 int SO3MathsQuatGetXaxis(mmachine m)
02047 {
02048         #ifdef SO3_DEBUG
02049         MMechostr(MSKDEBUG,"SO3MathsQuatGetXaxis\n");
02050         #endif
02051 
02052         int q = MMget(m, 0);
02053         if(q==NIL)
02054   {
02055     MMset(m, 0, NIL);
02056     return 0;
02057   }
02058   
02059   int x = MMfetch(m, MTOP(q), 0);
02060   int y = MMfetch(m, MTOP(q), 1);
02061   int z = MMfetch(m, MTOP(q), 2);
02062   int w = MMfetch(m, MTOP(q), 3);
02063 
02064         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
02065   {
02066     MMset(m, 0, NIL);
02067     return 0;   
02068   }
02069 
02070   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
02071   quat.normalise();
02072 
02073         Ogre::Vector3 vec = quat.xAxis();
02074   
02075         int tuple = MMmalloc(m, 3, TYPETAB);
02076         if(tuple==NIL)  
02077   {
02078     MMset(m, 0, NIL);   
02079     return MERRMEM;
02080   }
02081         MMstore(m, tuple, 0, FTOM((vec.x)));
02082         MMstore(m, tuple, 1, FTOM((vec.y)));
02083         MMstore(m, tuple, 2, FTOM((vec.z)));
02084         MMset(m, 0, PTOM(tuple));
02085 
02086         return 0;
02087 }
02088 
02089 
02098 int SO3MathsQuatGetYaxis(mmachine m)
02099 {
02100         #ifdef SO3_DEBUG
02101         MMechostr(MSKDEBUG,"SO3MathsQuatGetYaxis\n");
02102         #endif
02103 
02104         int q = MTOP(MMget(m, 0));
02105         if(q==NIL)                      
02106   {
02107     MMset(m, 0, NIL);
02108     return 0;
02109   }
02110   
02111   int x = MMfetch(m, q, 0);
02112   int y = MMfetch(m, q, 1);
02113   int z = MMfetch(m, q, 2);
02114   int w = MMfetch(m, q, 3);
02115 
02116         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
02117   {
02118     MMset(m, 0, NIL);
02119     return 0;   
02120   }
02121 
02122   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
02123   quat.normalise();
02124 
02125         Ogre::Vector3 vec = quat.yAxis();
02126 
02127         int tuple = MMmalloc(m, 3, TYPETAB);
02128         if(tuple==NIL)  
02129   {
02130     MMset(m, 0, NIL);   
02131     return MERRMEM;
02132   }
02133         MMstore(m, tuple, 0, FTOM((vec.x)));
02134         MMstore(m, tuple, 1, FTOM((vec.y)));
02135         MMstore(m, tuple, 2, FTOM((vec.z)));
02136         MMset(m, 0, PTOM(tuple));
02137 
02138         return 0;
02139 }
02140 
02141 
02150 int SO3MathsQuatGetZaxis(mmachine m)
02151 {
02152         #ifdef SO3_DEBUG
02153         MMechostr(MSKDEBUG,"SO3MathsQuatGetZaxis\n");
02154         #endif
02155 
02156         int q = MTOP(MMget(m, 0));
02157         if(q==NIL)                      
02158   {
02159     MMset(m, 0, NIL);
02160     return 0;
02161   }
02162   
02163   int x = MMfetch(m, q, 0);
02164   int y = MMfetch(m, q, 1);
02165   int z = MMfetch(m, q, 2);
02166   int w = MMfetch(m, q, 3);
02167 
02168         if((x==NIL) || (y==NIL) || (z==NIL) || (w==NIL))
02169   {
02170     MMset(m, 0, NIL);
02171     return 0;   
02172   }
02173 
02174   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
02175         quat.normalise();
02176 
02177   Ogre::Vector3 vec = quat.zAxis();
02178 
02179         int tuple = MMmalloc(m, 3, TYPETAB);
02180         if(tuple==NIL)  
02181   {
02182     MMset(m, 0, NIL);   
02183     return MERRMEM;
02184   }
02185         MMstore(m, tuple, 0, FTOM((vec.x)));
02186         MMstore(m, tuple, 1, FTOM((vec.y)));
02187         MMstore(m, tuple, 2, FTOM((vec.z)));
02188         MMset(m, 0, PTOM(tuple));
02189 
02190         return 0;
02191 }
02192 
02193 
02203 int SO3MathsQuatGetDirection(mmachine m)
02204 {
02205         #ifdef SO3_DEBUG
02206         MMechostr(MSKDEBUG,"SO3MathsQuatGetDirection\n");
02207         #endif
02208         
02209         int v = MTOP(MMpull(m));
02210         int q = MTOP(MMget(m, 0));
02211 
02212         if(v==NIL || q==NIL)                            
02213   {
02214     MMset(m,0,NIL);
02215     return 0;   
02216   }
02217 
02218         int vx = MMfetch(m, v, 0);
02219   int vy = MMfetch(m, v, 1);
02220   int vz = MMfetch(m, v, 2);
02221 
02222   if((vx==NIL) || (vy==NIL) || (vz==NIL))
02223   {
02224     MMset(m, 0, NIL);
02225     return 0;   
02226   }
02227   
02228   Ogre::Vector3 vec = Ogre::Vector3(MTOF(vx), MTOF(vy), MTOF(vz));
02229 
02230   int qx = MMfetch(m, q, 0);
02231   int qy = MMfetch(m, q, 1);
02232   int qz = MMfetch(m, q, 2);
02233   int qw = MMfetch(m, q, 3);
02234 
02235         if((qx==NIL) || (qy==NIL) || (qz==NIL) || (qw==NIL))
02236   {
02237     MMset(m, 0, NIL);
02238     return 0;   
02239   }
02240 
02241   Ogre::Quaternion quat = Ogre::Quaternion(MTOF(qw), MTOF(qx), MTOF(qy), MTOF(qz));
02242   quat.normalise();
02243 
02244         Ogre::Vector3 rvec = quat * vec;
02245 
02246         int tuple = MMmalloc(m, 3, TYPETAB);
02247         if(tuple==NIL)
02248   {     
02249     MMset(m,0,NIL);
02250     return MERRMEM;
02251   }
02252         MMstore(m, tuple, 0, FTOM((rvec.x)));
02253         MMstore(m, tuple, 1, FTOM((rvec.y)));
02254         MMstore(m, tuple, 2, FTOM((rvec.z)));
02255         MMset(m,0,PTOM(tuple));
02256 
02257         return 0;
02258 }
02259 
02260 
02269 int SO3MathsClampValue(mmachine m)
02270 {
02271 #ifdef SO3_DEBUG
02272         MMechostr(MSKDEBUG,"SO3MathsClampValue\n");
02273 #endif
02274 
02275         int valMax = MMpull(m);
02276         int valMin = MMpull(m);
02277         int val = MMget(m,0);
02278 
02279         if((val==NIL) || (valMin==NIL) || (valMax==NIL))                        
02280   {     
02281     MMset(m, 0, NIL);
02282     return 0;           
02283   }
02284 
02285         float result = Ogre::Math::Clamp(MTOF(val), MTOF(valMin), MTOF(valMax));
02286 
02287         MMset(m,0,FTOM(result));
02288 
02289         return 0;
02290 }
02291 
02292 
02302 int SO3MathsGetRotationTo(mmachine m)
02303 {
02304         #ifdef SO3_DEBUG
02305         MMechostr(MSKDEBUG,"SO3MathsGetRotationTo\n");
02306         #endif
02307 
02308         int scolVector2 = MTOP(MMpull(m));
02309         int scolVector1 = MTOP(MMget(m, 0));
02310         if((scolVector1==NIL) || (scolVector2==NIL))                            
02311   {     
02312     MMset(m, 0, NIL);           
02313     return 0;           
02314   }
02315 
02316   // Get the first vector from Scol VM.
02317   int xTemp = MMfetch(m, scolVector1, 0);
02318   int yTemp = MMfetch(m, scolVector1, 1);
02319   int zTemp = MMfetch(m, scolVector1, 2);
02320         if((xTemp == NIL)
02321    ||(yTemp == NIL)
02322    ||(zTemp == NIL))
02323   {     
02324     MMset(m, 0, NIL);
02325     return 0;   
02326   }
02327   Ogre::Vector3 sourceVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
02328 
02329   // Get the second vector from Scol VM.
02330   xTemp = MMfetch(m, scolVector2, 0);
02331   yTemp = MMfetch(m, scolVector2, 1);
02332   zTemp = MMfetch(m, scolVector2, 2);
02333         if((xTemp == NIL)
02334    ||(yTemp == NIL)
02335    ||(zTemp == NIL))
02336   {     
02337     MMset(m, 0, NIL);           
02338     return 0;
02339   }
02340   Ogre::Vector3 destinationVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
02341 
02342   // Compute quaternion to apply to go from source axes to destination axes.
02343   Ogre::Quaternion quatResult = sourceVector.getRotationTo(destinationVector);
02344   quatResult.normalise();
02345 
02346   // Send result to Scol VM.
02347         int tuple = MMmalloc(m, 4, TYPETAB);
02348         if(tuple==NIL)
02349   {     
02350     MMset(m,0,NIL);
02351     return MERRMEM;             
02352   }
02353   MMstore(m, tuple, 0, FTOM((quatResult.x)));
02354         MMstore(m, tuple, 1, FTOM((quatResult.y)));
02355         MMstore(m, tuple, 2, FTOM((quatResult.z)));
02356         MMstore(m, tuple, 3, FTOM((quatResult.w)));
02357   MMset(m,0,PTOM(tuple));
02358         return 0;
02359 }
02360 
02361 
02375 int SO3MathsVectorDotProduct(mmachine m)
02376 {
02377 #ifdef SO3_DEBUG
02378         MMechostr(MSKDEBUG, "SO3MathsVectorDotProduct\n");
02379 #endif
02380 
02381         int scolVector2 = MTOP(MMpull(m));
02382         int scolVector1 = MTOP(MMget(m, 0));
02383         if((scolVector1==NIL) || (scolVector2==NIL))                            
02384   {     
02385     MMset(m, 0, NIL);           
02386     return 0;           
02387   }
02388 
02389   // Get the first vector from Scol VM.
02390   int xTemp = MMfetch(m, scolVector1, 0);
02391   int yTemp = MMfetch(m, scolVector1, 1);
02392   int zTemp = MMfetch(m, scolVector1, 2);
02393         if((xTemp == NIL)
02394    ||(yTemp == NIL)
02395    ||(zTemp == NIL))
02396   {     
02397     MMset(m, 0, NIL);
02398     return 0;   
02399   }
02400   Ogre::Vector3 referenceVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
02401 
02402   // Get the second vector from Scol VM.
02403   xTemp = MMfetch(m, scolVector2, 0);
02404   yTemp = MMfetch(m, scolVector2, 1);
02405   zTemp = MMfetch(m, scolVector2, 2);
02406         if((xTemp == NIL)
02407    ||(yTemp == NIL)
02408    ||(zTemp == NIL))
02409   {     
02410     MMset(m, 0, NIL);           
02411     return 0;
02412   }
02413   Ogre::Vector3 destinationVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
02414 
02415   // Compute dot product.
02416   Ogre::Real scalarProduct = referenceVector.dotProduct(destinationVector);
02417 
02418   // Send result to Scol VM.
02419   MMset(m, 0, FTOM(scalarProduct));
02420         return 0;
02421 }
02422 
02423 
02427 #define SO3MathsNbPKG   42
02428 
02429 
02433 char* SO3MathsName[SO3MathsNbPKG]=
02434 {
02435         "SO3MathsQuatToEulerXYZ",
02436         "SO3MathsQuatToEulerXZY",
02437         "SO3MathsQuatToEulerYXZ",
02438         "SO3MathsQuatToEulerYZX",
02439         "SO3MathsQuatToEulerZXY",
02440         "SO3MathsQuatToEulerZYX",
02441         "SO3MathsQuatToEulerDegreeXYZ",
02442         "SO3MathsQuatToEulerDegreeXZY",
02443         "SO3MathsQuatToEulerDegreeYXZ",
02444         "SO3MathsQuatToEulerDegreeYZX",
02445         "SO3MathsQuatToEulerDegreeZXY",
02446         "SO3MathsQuatToEulerDegreeZYX",
02447         "SO3MathsEulerXYZToQuat",
02448         "SO3MathsEulerXZYToQuat",
02449         "SO3MathsEulerYXZToQuat",
02450         "SO3MathsEulerYZXToQuat",
02451         "SO3MathsEulerZYXToQuat",
02452         "SO3MathsEulerZXYToQuat",
02453         "SO3MathsQuatDiff",
02454         "SO3MathsQuatSubstract",
02455         "SO3MathsQuatAdd",
02456         "SO3MathsDegreeToRadian",
02457         "SO3MathsRadianToDegree",
02458         "SO3MathsQuatGetRoll",
02459         "SO3MathsQuatGetYaw",
02460         "SO3MathsQuatGetPitch",
02461         "SO3MathsQuatGetDegreeRoll",
02462         "SO3MathsQuatGetDegreeYaw",
02463         "SO3MathsQuatGetDegreePitch",
02464         "SO3MathsQuatToAxes",
02465         "SO3MathsQuatFromAxes",
02466         "SO3MathsQuatGetXaxis",
02467         "SO3MathsQuatGetYaxis",
02468         "SO3MathsQuatGetZaxis",
02469         "SO3MathsQuatInterpolate",
02470         "SO3MathsQuatGetDirection",
02471   "SO3MathsQuatFromAngle",
02472   "SO3MathsQuatFromDegreeAngle",
02473   "SO3MathsQuatFromAngleAxis",
02474   "SO3MathsGetRotationTo",
02475         "SO3MathsClampValue",
02476   "SO3MathsVectorDotProduct"
02477 };
02478 
02479 
02483 int (*SO3MathsFunc[SO3MathsNbPKG])(mmachine m) =
02484 {
02485         SO3MathsQuatToEulerXYZ,
02486         SO3MathsQuatToEulerXZY,
02487         SO3MathsQuatToEulerYXZ,
02488         SO3MathsQuatToEulerYZX,
02489         SO3MathsQuatToEulerZXY,
02490         SO3MathsQuatToEulerZYX,
02491         SO3MathsQuatToEulerDegreeXYZ,
02492         SO3MathsQuatToEulerDegreeXZY,
02493         SO3MathsQuatToEulerDegreeYXZ,
02494         SO3MathsQuatToEulerDegreeYZX,
02495         SO3MathsQuatToEulerDegreeZXY,
02496         SO3MathsQuatToEulerDegreeZYX,
02497         SO3MathsEulerXYZToQuat,
02498         SO3MathsEulerXZYToQuat,
02499         SO3MathsEulerYXZToQuat,
02500         SO3MathsEulerYZXToQuat,
02501         SO3MathsEulerZYXToQuat,
02502         SO3MathsEulerZXYToQuat,
02503         SO3MathsQuatDiff,
02504   SO3MathsQuatSubstract,
02505         SO3MathsQuatAdd,
02506         SO3MathsDegreeToRadian,
02507         SO3MathsRadianToDegree,
02508         SO3MathsQuatGetRoll,
02509         SO3MathsQuatGetYaw,
02510         SO3MathsQuatGetPitch,
02511         SO3MathsQuatGetDegreeRoll,
02512         SO3MathsQuatGetDegreeYaw,
02513         SO3MathsQuatGetDegreePitch,
02514         SO3MathsQuatToAxes,
02515         SO3MathsQuatFromAxes,
02516         SO3MathsQuatGetXaxis,
02517         SO3MathsQuatGetYaxis,
02518         SO3MathsQuatGetZaxis,
02519         SO3MathsQuatInterpolate,
02520         SO3MathsQuatGetDirection,
02521   SO3MathsQuatFromAngle,
02522   SO3MathsQuatFromDegreeAngle,
02523   SO3MathsQuatFromAngleAxis,
02524   SO3MathsGetRotationTo,
02525         SO3MathsClampValue,
02526   SO3MathsVectorDotProduct
02527 };
02528 
02529 
02533 int SO3MathsNArg[SO3MathsNbPKG]=
02534 {
02535         1,                                                                                                                                                                        // SO3MathsQuatToEulerXYZ
02536         1,                                                                                                                                                                        // SO3MathsQuatToEulerXZY
02537         1,                                                                                                                                                                        // SO3MathsQuatToEulerYXZ
02538         1,                                                                                                                                                                        // SO3MathsQuatToEulerYZX
02539         1,                                                                                                                                                                        // SO3MathsQuatToEulerZXY
02540         1,                                                                                                                                                                        // SO3MathsQuatToEulerZYX
02541         1,                                                                                                                                                                        // SO3MathsQuatToEulerDegreeXYZ
02542         1,                                                                                                                                                                        // SO3MathsQuatToEulerDegreeXZY
02543         1,                                                                                                                                                                        // SO3MathsQuatToEulerDegreeYXZ
02544         1,                                                                                                                                                                        // SO3MathsQuatToEulerDegreeYZX
02545         1,                                                                                                                                                                        // SO3MathsQuatToEulerDegreeZXY
02546         1,                                                                                                                                                                        // SO3MathsQuatToEulerDegreeZYX
02547         1,                                                                                                                                                                        // SO3MathsEulerXYZToQuat
02548         1,                                                                                                                                                                        // SO3MathsEulerXZYToQuat
02549         1,                                                                                                                                                                        // SO3MathsEulerYXZToQuat
02550         1,                                                                                                                                                                        // SO3MathsEulerYZXToQuat
02551         1,                                                                                                                                                                        // SO3MathsEulerZXYToQuat
02552         1,                                                                                                                                                                        // SO3MathsEulerZYXToQuat
02553         2,                                                                                                                                                                        // SO3MathsQuatDiff
02554         2,                                                                                                                                                                        // SO3MathsQuatSubstract
02555         2,                                                                                                                                                                        // SO3MathsQuatAdd
02556         1,                                                                                                                                                                        // SO3MathsDegreeToRadian
02557         1,                                                                                                                                                                        // SO3MathsRadianToDegree
02558         2,                                                                                                                                                                        // SO3MathsQuatGetRoll
02559         2,                                                                                                                                                                        // SO3MathsQuatGetYaw
02560         2,                                                                                                                                                                        // SO3MathsQuatGetPitch
02561         2,                                                                                                                                                                        // SO3MathsQuatGetDegreeRoll
02562         2,                                                                                                                                                                        // SO3MathsQuatGetDegreeYaw
02563         2,                                                                                                                                                                        // SO3MathsQuatGetDegreePitch
02564         1,                                                                                                                                                                        // SO3MathsQuatToAxes
02565         3,                                                                                                                                                                        // SO3MathsQuatFromAxes
02566         1,                                                                                                                                                                        // SO3MathsQuatGetXaxis
02567         1,                                                                                                                                                                        // SO3MathsQuatGetYaxis
02568         1,                                                                                                                                                                        // SO3MathsQuatGetZaxis
02569         4,                                                                                                                                                                        // SO3MathsQuatInterpolate
02570         2,                                                                                                                                                                        // SO3MathsQuatGetDirection
02571   1,                                            // SO3MathsQuatFromAngle
02572   1,                                            // SO3MathsQuatFromDegreeAngle
02573   2,                                            // SO3MathsQuatFromAngleAxis
02574   2,                                            // SO3MathsGetRotationTo
02575         3,                                                                                                                                                                        // SO3MathsClampValue,
02576   2                                             // SO3MathsVectorDotProduct
02577  };
02578 
02579 
02583 char* SO3MathsType[SO3MathsNbPKG]=
02584 {
02585         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerXYZ
02586         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerXZY
02587         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerYXZ
02588         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerYZX
02589         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerZXY
02590         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerZYX
02591         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerDegreeXYZ
02592         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerDegreeXZY
02593         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerDegreeYXZ
02594         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerDegreeYZX
02595         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerDegreeZXY
02596         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatToEulerDegreeZYX
02597         "fun [[F F F]] [F F F F]",                                                                        // SO3MathsEulerXYZToQuat
02598         "fun [[F F F]] [F F F F]",                                                                        // SO3MathsEulerXZYToQuat
02599         "fun [[F F F]] [F F F F]",                                                                        // SO3MathsEulerYXZToQuat
02600         "fun [[F F F]] [F F F F]",                                                                        // SO3MathsEulerYZXToQuat
02601         "fun [[F F F]] [F F F F]",                                                                        // SO3MathsEulerZXYToQuat
02602         "fun [[F F F]] [F F F F]",                                                                        // SO3MathsEulerZYXToQuat
02603         "fun [[F F F F] [F F F F]] [F F F F]",                    // SO3MathsQuatDiff
02604   "fun [[F F F F] [F F F F]] [F F F F]",                          // SO3MathsQuatSubstract
02605         "fun [[F F F F] [F F F F]] [F F F F]",                    // SO3MathsQuatAdd
02606         "fun [F] F",                                                                                                                              // SO3MathsDegreeToRadian
02607         "fun [F] F",                                                                                                                              // SO3MathsRadianToDegree
02608         "fun [[F F F F] I] F",                                                                                    // SO3MathsQuatGetRoll
02609         "fun [[F F F F] I] F",                                                                                    // SO3MathsQuatGetYaw
02610         "fun [[F F F F] I] F",                                                                                    // SO3MathsQuatGetPitch
02611         "fun [[F F F F] I] F",                                                                                    // SO3MathsQuatGetDegreeRoll
02612         "fun [[F F F F] I] F",                                                                                    // SO3MathsQuatGetDegreeYaw
02613         "fun [[F F F F] I] F",                                                                                    // SO3MathsQuatGetDegreePitch
02614         "fun [[F F F F]] [[F F F] [F F F] [F F F]]",  // SO3MathsQuatToAxes
02615         "fun [[F F F] [F F F] [F F F]] [F F F F]",        // SO3MathsQuatFromAxes
02616         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatGetXaxis
02617         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatGetYaxis
02618         "fun [[F F F F]] [F F F]",                                                                        // SO3MathsQuatGetZaxis
02619         "fun [[F F F F] [F F F F] F I] [F F F F]",        // SO3MathsQuatInterpolate
02620         "fun [[F F F F] [F F F]] [F F F]",                                        // SO3MathsQuatGetDirection
02621   "fun [[F F F]] [F F F F]",                    // SO3MathsQuatFromAngle
02622   "fun [[F F F]] [F F F F]",                    // SO3MathsQuatFromDegreeAngle
02623   "fun [F [F F F]] [F F F F]",                  // SO3MathsQuatFromAngleAxis
02624   "fun [[F F F] [F F F]] [F F F F]",            // SO3MathsGetRotationTo
02625         "fun [F F F] F",                                                                                                                  // SO3MathsClampValue
02626   "fun [[F F F] [F F F]] F"                     // SO3MathsVectorDotProduct
02627 };
02628 
02629 
02635 int SCOLloadMaths(mmachine m,cbmachine w)
02636 {
02637   return PKhardpak(m, "SO3Maths", SO3MathsNbPKG, SO3MathsName, SO3MathsFunc, SO3MathsNArg, SO3MathsType);
02638 }
02639 
02640 
02645 int SCOLfreeMaths()
02646 {
02647   return 0;
02648 }