SCOLMaths.cpp

Go to the documentation of this file.

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com
 
Copyright (c) 2012 I-maginer
 
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
 
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
 
You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt
 
-----------------------------------------------------------------------------
*/
 
 
#include "SCOLPack/SO3SCOL.h"
 
// Scene Graph includes
#include "SO3SceneGraph/SO3NodeScol.h"
 
#include "SO3Utils/SO3Euler.h"
 
 
int SO3MathsQuatToEulerXYZ(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerXYZ\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesXYZ(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
  MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
  MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerXZY(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerXZY\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesXZY(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
  MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
  MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerYXZ(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerYXZ\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesYXZ(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
  MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
  MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerYZX(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerYZX\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
  mat.ToEulerAnglesYZX(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
  MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
  MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
  MMset(m, 0, PTOM(tuple));
  return 0;
}
 
 
int SO3MathsQuatToEulerZXY(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerZXY\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesZXY(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
  MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
  MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerZYX(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerZYX\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
 
  quat.normalise();
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesZYX(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
  MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
  MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerDegreeXYZ(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerDegreeXYZ\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesXYZ(yrad, xrad, zrad);
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
  MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
  MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerDegreeXZY(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerDegreeXZY\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesXZY(yrad, xrad, zrad);
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
  MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
  MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerDegreeYXZ(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerDegreeYXZ\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesYXZ(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
  MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
  MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerDegreeYZX(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerDegreeYZX\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesYZX(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
  MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
  MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerDegreeZXY(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerDegreeZXY\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesZXY(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
  MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
  MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatToEulerDegreeZYX(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerDegreeZYX\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Matrix3 mat;
  Ogre::Radian xrad;
  Ogre::Radian yrad;
  Ogre::Radian zrad;
 
  quat.ToRotationMatrix(mat);
 
  mat.ToEulerAnglesZYX(yrad, xrad, zrad);
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
  MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
  MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsEulerXYZToQuat(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsEulerXYZToQuat\n");
#endif
 
  int vec = MTOP(MMget(m, 0));
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, vec, 0);
  int y = MMfetch(m, vec, 1);
  int z = MMfetch(m, vec, 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Radian xrad = Ogre::Radian(MTOF(x));
  Ogre::Radian yrad = Ogre::Radian(MTOF(y));
  Ogre::Radian zrad = Ogre::Radian(MTOF(z));
 
  Ogre::Matrix3 mat;
 
  mat.FromEulerAnglesXYZ(yrad, xrad, zrad);
 
  Ogre::Quaternion quat;
  quat.FromRotationMatrix(mat);
  quat.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quat.x));
  MMstore(m, tuple, 1, FTOM(quat.y));
  MMstore(m, tuple, 2, FTOM(quat.z));
  MMstore(m, tuple, 3, FTOM(quat.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsEulerXZYToQuat(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsEulerXZYToQuat\n");
#endif
 
  int vec = MTOP(MMget(m, 0));
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, vec, 0);
  int y = MMfetch(m, vec, 1);
  int z = MMfetch(m, vec, 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Radian xrad = Ogre::Radian(MTOF(x));
  Ogre::Radian yrad = Ogre::Radian(MTOF(y));
  Ogre::Radian zrad = Ogre::Radian(MTOF(z));
 
  Ogre::Matrix3 mat;
 
  mat.FromEulerAnglesXZY(yrad, xrad, zrad);
 
  Ogre::Quaternion quat;
  quat.FromRotationMatrix(mat);
  quat.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quat.x));
  MMstore(m, tuple, 1, FTOM(quat.y));
  MMstore(m, tuple, 2, FTOM(quat.z));
  MMstore(m, tuple, 3, FTOM(quat.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsEulerYXZToQuat(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsEulerYXZToQuat\n");
#endif
 
  int vec = MTOP(MMget(m, 0));
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, vec, 0);
  int y = MMfetch(m, vec, 1);
  int z = MMfetch(m, vec, 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Radian xrad = Ogre::Radian(MTOF(x));
  Ogre::Radian yrad = Ogre::Radian(MTOF(y));
  Ogre::Radian zrad = Ogre::Radian(MTOF(z));
 
  Ogre::Matrix3 mat;
 
  mat.FromEulerAnglesYXZ(yrad, xrad, zrad);
 
  Ogre::Quaternion quat;
  quat.FromRotationMatrix(mat);
  quat.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quat.x));
  MMstore(m, tuple, 1, FTOM(quat.y));
  MMstore(m, tuple, 2, FTOM(quat.z));
  MMstore(m, tuple, 3, FTOM(quat.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsEulerYZXToQuat(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsEulerYZXToQuat\n");
#endif
 
  int vec = MTOP(MMget(m, 0));
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, vec, 0);
  int y = MMfetch(m, vec, 1);
  int z = MMfetch(m, vec, 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Radian xrad = Ogre::Radian(MTOF(x));
  Ogre::Radian yrad = Ogre::Radian(MTOF(y));
  Ogre::Radian zrad = Ogre::Radian(MTOF(z));
 
  Ogre::Matrix3 mat;
 
  mat.FromEulerAnglesYZX(yrad, xrad, zrad);
 
  Ogre::Quaternion quat;
  quat.FromRotationMatrix(mat);
  quat.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quat.x));
  MMstore(m, tuple, 1, FTOM(quat.y));
  MMstore(m, tuple, 2, FTOM(quat.z));
  MMstore(m, tuple, 3, FTOM(quat.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsEulerZYXToQuat(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsEulerZYXToQuat\n");
#endif
 
  int vec = MTOP(MMget(m, 0));
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, vec, 0);
  int y = MMfetch(m, vec, 1);
  int z = MMfetch(m, vec, 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Radian xrad = Ogre::Radian(MTOF(x));
  Ogre::Radian yrad = Ogre::Radian(MTOF(y));
  Ogre::Radian zrad = Ogre::Radian(MTOF(z));
 
  Ogre::Matrix3 mat;
 
  mat.FromEulerAnglesZYX(yrad, xrad, zrad);
 
  Ogre::Quaternion quat;
  quat.FromRotationMatrix(mat);
  quat.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quat.x));
  MMstore(m, tuple, 1, FTOM(quat.y));
  MMstore(m, tuple, 2, FTOM(quat.z));
  MMstore(m, tuple, 3, FTOM(quat.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsEulerZXYToQuat(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsEulerZXYToQuat\n");
#endif
 
  int vec = MTOP(MMget(m, 0));
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, vec, 0);
  int y = MMfetch(m, vec, 1);
  int z = MMfetch(m, vec, 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Radian xrad = Ogre::Radian(MTOF(x));
  Ogre::Radian yrad = Ogre::Radian(MTOF(y));
  Ogre::Radian zrad = Ogre::Radian(MTOF(z));
 
  Ogre::Matrix3 mat;
 
  mat.FromEulerAnglesZXY(yrad, xrad, zrad);
 
  Ogre::Quaternion quat;
  quat.FromRotationMatrix(mat);
  quat.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quat.x));
  MMstore(m, tuple, 1, FTOM(quat.y));
  MMstore(m, tuple, 2, FTOM(quat.z));
  MMstore(m, tuple, 3, FTOM(quat.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatIsEqual(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatIsEqual\n");
#endif
 
  int q2 = MTOP(MMpull(m));
  int q1 = MTOP(MMget(m, 0));
 
  if (q1 == NIL || q2 == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x1 = MMfetch(m, q1, 0);
  int y1 = MMfetch(m, q1, 1);
  int z1 = MMfetch(m, q1, 2);
  int w1 = MMfetch(m, q1, 3);
 
  int x2 = MMfetch(m, q2, 0);
  int y2 = MMfetch(m, q2, 1);
  int z2 = MMfetch(m, q2, 2);
  int w2 = MMfetch(m, q2, 3);
 
  if ((x1 == NIL) || (y1 == NIL) || (z1 == NIL) || (w1 == NIL) || (x2 == NIL) || (y2 == NIL) || (z2 == NIL) || (w2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
  Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
 
  MMset(m, 0, ITOM((quat1 == quat2 ? 1 : 0)));
 
  return 0;
}
 
int SO3MathsQuatDiff(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatDiff\n");
#endif
 
  int q2 = MTOP(MMpull(m));
  int q1 = MTOP(MMget(m, 0));
 
  if (q1 == NIL || q2 == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x1 = MMfetch(m, q1, 0);
  int y1 = MMfetch(m, q1, 1);
  int z1 = MMfetch(m, q1, 2);
  int w1 = MMfetch(m, q1, 3);
 
  int x2 = MMfetch(m, q2, 0);
  int y2 = MMfetch(m, q2, 1);
  int z2 = MMfetch(m, q2, 2);
  int w2 = MMfetch(m, q2, 3);
 
  if ((x1 == NIL) || (y1 == NIL) || (z1 == NIL) || (w1 == NIL) || (x2 == NIL) || (y2 == NIL) || (z2 == NIL) || (w2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
  Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
 
  Ogre::Quaternion quatResult = quat1 * quat2.Inverse();
  quatResult.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quatResult.x));
  MMstore(m, tuple, 1, FTOM(quatResult.y));
  MMstore(m, tuple, 2, FTOM(quatResult.z));
  MMstore(m, tuple, 3, FTOM(quatResult.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatSubstract(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatSubstract\n");
#endif
 
  int q2 = MTOP(MMpull(m));
  int q1 = MTOP(MMget(m, 0));
 
  if (q1 == NIL || q2 == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x1 = MMfetch(m, q1, 0);
  int y1 = MMfetch(m, q1, 1);
  int z1 = MMfetch(m, q1, 2);
  int w1 = MMfetch(m, q1, 3);
 
  int x2 = MMfetch(m, q2, 0);
  int y2 = MMfetch(m, q2, 1);
  int z2 = MMfetch(m, q2, 2);
  int w2 = MMfetch(m, q2, 3);
 
  if ((x1 == NIL) || (y1 == NIL) || (z1 == NIL) || (w1 == NIL) || (x2 == NIL) || (y2 == NIL) || (z2 == NIL) || (w2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
 
  Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
 
  Ogre::Quaternion quatResult = quat1.Inverse() * quat2;
  quatResult.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quatResult.x));
  MMstore(m, tuple, 1, FTOM(quatResult.y));
  MMstore(m, tuple, 2, FTOM(quatResult.z));
  MMstore(m, tuple, 3, FTOM(quatResult.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatAdd(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatAdd\n");
#endif
 
  int q2 = MTOP(MMpull(m));
  int q1 = MTOP(MMget(m, 0));
 
  if (q1 == NIL || q2 == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x1 = MMfetch(m, q1, 0);
  int y1 = MMfetch(m, q1, 1);
  int z1 = MMfetch(m, q1, 2);
  int w1 = MMfetch(m, q1, 3);
 
  int x2 = MMfetch(m, q2, 0);
  int y2 = MMfetch(m, q2, 1);
  int z2 = MMfetch(m, q2, 2);
  int w2 = MMfetch(m, q2, 3);
 
  if ((x1 == NIL) || (y1 == NIL) || (z1 == NIL) || (w1 == NIL) || (x2 == NIL) || (y2 == NIL) || (z2 == NIL) || (w2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
  quat1.normalise();
  Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
  quat2.normalise();
 
  Ogre::Quaternion quatResult = quat1 * quat2;
  quatResult.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
 
  MMstore(m, tuple, 0, FTOM(quatResult.x));
  MMstore(m, tuple, 1, FTOM(quatResult.y));
  MMstore(m, tuple, 2, FTOM(quatResult.z));
  MMstore(m, tuple, 3, FTOM(quatResult.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatInterpolate(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatInterpolate\n");
#endif
 
  int ishort = MTOI(MMpull(m));
  int coef = MMpull(m);
  int q2 = MTOP(MMpull(m));
  int q1 = MTOP(MMget(m, 0));
 
  if ((q1 == NIL) || (q2 == NIL) || (coef == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  bool bshort = false;
  if (ishort == 1)
    bshort = true;
 
  int x1 = MMfetch(m, q1, 0);
  int y1 = MMfetch(m, q1, 1);
  int z1 = MMfetch(m, q1, 2);
  int w1 = MMfetch(m, q1, 3);
 
  int x2 = MMfetch(m, q2, 0);
  int y2 = MMfetch(m, q2, 1);
  int z2 = MMfetch(m, q2, 2);
  int w2 = MMfetch(m, q2, 3);
 
  if ((x1 == NIL) || (y1 == NIL) || (z1 == NIL) || (w1 == NIL) || (x2 == NIL) || (y2 == NIL) || (z2 == NIL) || (w2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat1 = Ogre::Quaternion(MTOF(w1), MTOF(x1), MTOF(y1), MTOF(z1));
  quat1.normalise();
  Ogre::Quaternion quat2 = Ogre::Quaternion(MTOF(w2), MTOF(x2), MTOF(y2), MTOF(z2));
  quat2.normalise();
 
  Ogre::Quaternion quatResult = Ogre::Quaternion::Slerp((Ogre::Real)MTOF(coef), quat1, quat2, bshort);
  quatResult.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
 
  MMstore(m, tuple, 0, FTOM(quatResult.x));
  MMstore(m, tuple, 1, FTOM(quatResult.y));
  MMstore(m, tuple, 2, FTOM(quatResult.z));
  MMstore(m, tuple, 3, FTOM(quatResult.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsDegreeToRadian(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsDegreeToRadian\n");
#endif
 
  int val = MMget(m, 0);
  if (val == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Degree deg = Ogre::Degree(MTOF(val));
  float rad = deg.valueRadians();
 
  MMset(m, 0, FTOM(rad));
 
  return 0;
}
 
 
int SO3MathsRadianToDegree(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsRadianToDegree\n");
#endif
 
  int val = MMget(m, 0);
  if (val == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Radian rad = Ogre::Radian(MTOF(val));
  float deg = rad.valueDegrees();
 
  MMset(m, 0, FTOM(deg));
 
  return 0;
}
 
 
int SO3MathsQuatGetRoll(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetRoll\n");
#endif
 
  int booleen = MTOI(MMpull(m));
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Radian roll = quat.getRoll(booleen == 1 ? true : false);
 
  MMset(m, 0, FTOM(roll.valueRadians()));
  return 0;
}
 
 
int SO3MathsQuatGetYaw(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetYaw\n");
#endif
 
  int booleen = MTOI(MMpull(m));
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Radian yaw = quat.getYaw(booleen == 1 ? true : false);
 
  MMset(m, 0, FTOM(yaw.valueRadians()));
  return 0;
}
 
 
int SO3MathsQuatGetPitch(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetPitch\n");
#endif
 
  int booleen = MTOI(MMpull(m));
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Radian pitch = quat.getPitch(booleen == 1 ? true : false);
 
  MMset(m, 0, FTOM(pitch.valueRadians()));
  return 0;
}
 
 
int SO3MathsQuatGetDegreeRoll(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetDegreeRoll\n");
#endif
 
  int booleen = MTOI(MMpull(m));
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Radian roll = quat.getRoll(booleen == 1 ? true : false);
 
  MMset(m, 0, FTOM(roll.valueDegrees()));
  return 0;
}
 
 
int SO3MathsQuatGetDegreeYaw(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetDegreeYaw\n");
#endif
 
  int booleen = MTOI(MMpull(m));
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Radian yaw = quat.getYaw(booleen == 1 ? true : false);
 
  MMset(m, 0, FTOM(yaw.valueDegrees()));
  return 0;
}
 
 
int SO3MathsQuatGetDegreePitch(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetDegreePitch\n");
#endif
 
  int booleen = MTOI(MMpull(m));
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Radian pitch = quat.getPitch(booleen == 1 ? true : false);
 
  MMset(m, 0, FTOM(pitch.valueDegrees()));
  return 0;
}
 
 
int SO3MathsQuatToAxes(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToAxes\n");
#endif
 
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Vector3 xAxis;
  Ogre::Vector3 yAxis;
  Ogre::Vector3 zAxis;
 
  quat.ToAxes(xAxis, yAxis, zAxis);
 
  // x
  int xtuple = MMmalloc(m, 3, TYPETAB);
  if (xtuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, xtuple, 0, FTOM(xAxis.x));
  MMstore(m, xtuple, 1, FTOM(xAxis.y));
  MMstore(m, xtuple, 2, FTOM(xAxis.z));
  MMpush(m, PTOM(xtuple));
 
  // y
  int ytuple = MMmalloc(m, 3, TYPETAB);
  if (ytuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, ytuple, 0, FTOM(yAxis.x));
  MMstore(m, ytuple, 1, FTOM(yAxis.y));
  MMstore(m, ytuple, 2, FTOM(yAxis.z));
  MMpush(m, PTOM(ytuple));
 
  // z
  int ztuple = MMmalloc(m, 3, TYPETAB);
  if (ztuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
 
  MMstore(m, ztuple, 0, FTOM(zAxis.x));
  MMstore(m, ztuple, 1, FTOM(zAxis.y));
  MMstore(m, ztuple, 2, FTOM(zAxis.z));
  MMpush(m, PTOM(ztuple));
 
  // FINAL TUPPLE 
  int result = MMmalloc(m, 3, TYPETAB);
  if (result == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, result, 2, MMpull(m));
  MMstore(m, result, 1, MMpull(m));
  MMstore(m, result, 0, MMpull(m));
  MMset(m, 0, PTOM(result));
 
  return 0;
}
 
int SO3MathsQuatFromAngle(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatFromAngle\n");
#endif
 
  int vec = MMget(m, 0);
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(vec), 0);
  int y = MMfetch(m, MTOP(vec), 1);
  int z = MMfetch(m, MTOP(vec), 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quatX;
  Ogre::Quaternion quatY;
  Ogre::Quaternion quatZ;
 
  quatX.FromAngleAxis(Ogre::Radian(MTOF(x)), Ogre::Vector3(1, 0, 0));
  quatY.FromAngleAxis(Ogre::Radian(MTOF(y)), Ogre::Vector3(0, 1, 0));
  quatZ.FromAngleAxis(Ogre::Radian(MTOF(z)), Ogre::Vector3(0, 0, 1));
 
  Ogre::Quaternion quatResult;
  quatResult = quatX * quatY * quatZ;
  quatResult.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((quatResult.x)));
  MMstore(m, tuple, 1, FTOM((quatResult.y)));
  MMstore(m, tuple, 2, FTOM((quatResult.z)));
  MMstore(m, tuple, 3, FTOM((quatResult.w)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
int SO3MathsQuatFromDegreeAngle(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatFromDegreeAngle\n");
#endif
 
  int vec = MMget(m, 0);
  if (vec == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(vec), 0);
  int y = MMfetch(m, MTOP(vec), 1);
  int z = MMfetch(m, MTOP(vec), 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quatX;
  Ogre::Quaternion quatY;
  Ogre::Quaternion quatZ;
 
  Ogre::Degree dx = (Ogre::Degree)MTOF(x);
  Ogre::Degree dy = (Ogre::Degree)MTOF(y);
  Ogre::Degree dz = (Ogre::Degree)MTOF(z);
  quatX.FromAngleAxis((Ogre::Radian)dx.valueRadians(), Ogre::Vector3(1, 0, 0));
  quatY.FromAngleAxis((Ogre::Radian)dy.valueRadians(), Ogre::Vector3(0, 1, 0));
  quatZ.FromAngleAxis((Ogre::Radian)dz.valueRadians(), Ogre::Vector3(0, 0, 1));
 
  Ogre::Quaternion quatResult;
  quatResult = quatX * quatY * quatZ;
  quatResult.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((quatResult.x)));
  MMstore(m, tuple, 1, FTOM((quatResult.y)));
  MMstore(m, tuple, 2, FTOM((quatResult.z)));
  MMstore(m, tuple, 3, FTOM((quatResult.w)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatFromAngleAxis(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatFromAngleAxis\n");
#endif
 
  int vec = MMpull(m);
  int rad = MMget(m, 0);
  if ((vec == NIL) || (rad == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(vec), 0);
  int y = MMfetch(m, MTOP(vec), 1);
  int z = MMfetch(m, MTOP(vec), 2);
 
  if ((x == NIL) || (y == NIL) || (z == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quatResult;
  Ogre::Vector3 axis(MTOF(x), MTOF(y), MTOF(z));
  quatResult.FromAngleAxis(Ogre::Radian(MTOF(rad)), axis);
  quatResult.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((quatResult.x)));
  MMstore(m, tuple, 1, FTOM((quatResult.y)));
  MMstore(m, tuple, 2, FTOM((quatResult.z)));
  MMstore(m, tuple, 3, FTOM((quatResult.w)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
int SO3MathsQuatFromAxes(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatFromAxes\n");
#endif
 
  int z = MTOP(MMpull(m));
  int y = MTOP(MMpull(m));
  int x = MTOP(MMget(m, 0));
  if (x == NIL || y == NIL || z == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Vector3 xAxis;
  xAxis.x = MTOF(MMfetch(m, x, 0));
  xAxis.y = MTOF(MMfetch(m, x, 1));
  xAxis.z = MTOF(MMfetch(m, x, 2));
 
  Ogre::Vector3 yAxis;
  yAxis.x = MTOF(MMfetch(m, y, 0));
  yAxis.y = MTOF(MMfetch(m, y, 1));
  yAxis.z = MTOF(MMfetch(m, y, 2));
 
  Ogre::Vector3 zAxis;
  zAxis.x = MTOF(MMfetch(m, z, 0));
  zAxis.y = MTOF(MMfetch(m, z, 1));
  zAxis.z = MTOF(MMfetch(m, z, 2));
 
 
  Ogre::Quaternion quat;
 
  quat.FromAxes(xAxis, yAxis, zAxis);
  quat.normalise();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((quat.x)));
  MMstore(m, tuple, 1, FTOM((quat.y)));
  MMstore(m, tuple, 2, FTOM((quat.z)));
  MMstore(m, tuple, 3, FTOM((quat.w)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatGetXaxis(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetXaxis\n");
#endif
 
  int q = MMget(m, 0);
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, MTOP(q), 0);
  int y = MMfetch(m, MTOP(q), 1);
  int z = MMfetch(m, MTOP(q), 2);
  int w = MMfetch(m, MTOP(q), 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Vector3 vec = quat.xAxis();
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((vec.x)));
  MMstore(m, tuple, 1, FTOM((vec.y)));
  MMstore(m, tuple, 2, FTOM((vec.z)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatGetYaxis(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetYaxis\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Vector3 vec = quat.yAxis();
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((vec.x)));
  MMstore(m, tuple, 1, FTOM((vec.y)));
  MMstore(m, tuple, 2, FTOM((vec.z)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatGetZaxis(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetZaxis\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  quat.normalise();
 
  Ogre::Vector3 vec = quat.zAxis();
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((vec.x)));
  MMstore(m, tuple, 1, FTOM((vec.y)));
  MMstore(m, tuple, 2, FTOM((vec.z)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsQuatGetDirection(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatGetDirection\n");
#endif
 
  int v = MTOP(MMpull(m));
  int q = MTOP(MMget(m, 0));
 
  if (v == NIL || q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int vx = MMfetch(m, v, 0);
  int vy = MMfetch(m, v, 1);
  int vz = MMfetch(m, v, 2);
 
  if ((vx == NIL) || (vy == NIL) || (vz == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Vector3 vec = Ogre::Vector3(MTOF(vx), MTOF(vy), MTOF(vz));
 
  int qx = MMfetch(m, q, 0);
  int qy = MMfetch(m, q, 1);
  int qz = MMfetch(m, q, 2);
  int qw = MMfetch(m, q, 3);
 
  if ((qx == NIL) || (qy == NIL) || (qz == NIL) || (qw == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(qw), MTOF(qx), MTOF(qy), MTOF(qz));
  quat.normalise();
 
  Ogre::Vector3 rvec = quat * vec;
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((rvec.x)));
  MMstore(m, tuple, 1, FTOM((rvec.y)));
  MMstore(m, tuple, 2, FTOM((rvec.z)));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
int SO3MathsClampValue(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsClampValue\n");
#endif
 
  int valMax = MMpull(m);
  int valMin = MMpull(m);
  int val = MMget(m, 0);
 
  if ((val == NIL) || (valMin == NIL) || (valMax == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  float result = Ogre::Math::Clamp(MTOF(val), MTOF(valMin), MTOF(valMax));
 
  MMset(m, 0, FTOM(result));
 
  return 0;
}
 
 
int SO3MathsGetRotationTo(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsGetRotationTo\n");
#endif
 
  int scolVector2 = MTOP(MMpull(m));
  int scolVector1 = MTOP(MMget(m, 0));
  if ((scolVector1 == NIL) || (scolVector2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  // Get the first vector from Scol VM.
  int xTemp = MMfetch(m, scolVector1, 0);
  int yTemp = MMfetch(m, scolVector1, 1);
  int zTemp = MMfetch(m, scolVector1, 2);
  if ((xTemp == NIL)
    || (yTemp == NIL)
    || (zTemp == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
  Ogre::Vector3 sourceVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
 
  // Get the second vector from Scol VM.
  xTemp = MMfetch(m, scolVector2, 0);
  yTemp = MMfetch(m, scolVector2, 1);
  zTemp = MMfetch(m, scolVector2, 2);
  if ((xTemp == NIL)
    || (yTemp == NIL)
    || (zTemp == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
  Ogre::Vector3 destinationVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
 
  // Compute quaternion to apply to go from source axes to destination axes.
  Ogre::Quaternion quatResult = sourceVector.getRotationTo(destinationVector);
  quatResult.normalise();
 
  // Send result to Scol VM.
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((quatResult.x)));
  MMstore(m, tuple, 1, FTOM((quatResult.y)));
  MMstore(m, tuple, 2, FTOM((quatResult.z)));
  MMstore(m, tuple, 3, FTOM((quatResult.w)));
  MMset(m, 0, PTOM(tuple));
  return 0;
}
 
 
int SO3MathsGetRotationToAxis(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsGetRotationToAxis\n");
#endif
 
  int scolVector2 = MTOP(MMpull(m));
  int scolVector1 = MTOP(MMget(m, 0));
  if ((scolVector1 == NIL) || (scolVector2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  // Get the first vector from Scol VM.
  int xTemp = MMfetch(m, scolVector1, 0);
  int yTemp = MMfetch(m, scolVector1, 1);
  int zTemp = MMfetch(m, scolVector1, 2);
  if ((xTemp == NIL)
    || (yTemp == NIL)
    || (zTemp == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
  Ogre::Vector3 src(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
 
  // Get the second vector from Scol VM.
  xTemp = MMfetch(m, scolVector2, 0);
  yTemp = MMfetch(m, scolVector2, 1);
  zTemp = MMfetch(m, scolVector2, 2);
  if ((xTemp == NIL)
    || (yTemp == NIL)
    || (zTemp == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
  Ogre::Vector3 axis(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
 
  //OrthoNormalize
  // N' = |N|
  // T' = |T - (N' dot T) N'|
  // B' = |B - (N' dot B) N' - (T' dot B) T'|
 
  Ogre::Vector3 forward = src;
  forward.normalise();
 
  Ogre::Vector3 up = axis;
  up -= forward * forward.dotProduct(up);
  up.normalise();
 
  Ogre::Vector3 right = up.crossProduct(forward);
  Ogre::Quaternion quatResult;
  quatResult.w = sqrtf(1.0f + right.x + up.y + forward.z) * 0.5f;
  float w4_recip = 1.0f / (4.0f * quatResult.w);
  quatResult.x = (up.z - forward.y) * w4_recip;
  quatResult.y = (forward.x - right.z) * w4_recip;
  quatResult.z = (right.y - up.x) * w4_recip;
 
  // Send result to Scol VM.
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((quatResult.x)));
  MMstore(m, tuple, 1, FTOM((quatResult.y)));
  MMstore(m, tuple, 2, FTOM((quatResult.z)));
  MMstore(m, tuple, 3, FTOM((quatResult.w)));
  MMset(m, 0, PTOM(tuple));
  return 0;
}
 
 
int SO3MathsVectorDotProduct(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsVectorDotProduct\n");
#endif
 
  int scolVector2 = MTOP(MMpull(m));
  int scolVector1 = MTOP(MMget(m, 0));
  if ((scolVector1 == NIL) || (scolVector2 == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  // Get the first vector from Scol VM.
  int xTemp = MMfetch(m, scolVector1, 0);
  int yTemp = MMfetch(m, scolVector1, 1);
  int zTemp = MMfetch(m, scolVector1, 2);
  if ((xTemp == NIL)
    || (yTemp == NIL)
    || (zTemp == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
  Ogre::Vector3 referenceVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
 
  // Get the second vector from Scol VM.
  xTemp = MMfetch(m, scolVector2, 0);
  yTemp = MMfetch(m, scolVector2, 1);
  zTemp = MMfetch(m, scolVector2, 2);
  if ((xTemp == NIL)
    || (yTemp == NIL)
    || (zTemp == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
  Ogre::Vector3 destinationVector(MTOF(xTemp), MTOF(yTemp), MTOF(zTemp));
 
  // Compute dot product.
  Ogre::Real scalarProduct = referenceVector.dotProduct(destinationVector);
 
  // Send result to Scol VM.
  MMset(m, 0, FTOM(scalarProduct));
  return 0;
}
 
 
int SO3MathsQuatRotate(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatRotate\n");
#endif
 
  int v = MTOP(MMpull(m));
  int q = MTOP(MMget(m, 0));
 
  if (v == NIL || q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int vyaw = MMfetch(m, v, 0);
  int vpitch = MMfetch(m, v, 1);
  int vroll = MMfetch(m, v, 2);
 
  if ((vyaw == NIL) || (vpitch == NIL) || (vroll == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int qx = MMfetch(m, q, 0);
  int qy = MMfetch(m, q, 1);
  int qz = MMfetch(m, q, 2);
  int qw = MMfetch(m, q, 3);
 
  if ((qx == NIL) || (qy == NIL) || (qz == NIL) || (qw == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(qw), MTOF(qx), MTOF(qy), MTOF(qz));
  Ogre::Euler euler(quat);
 
  euler.rotate(Ogre::Radian(MTOF(vyaw)), Ogre::Radian(MTOF(vpitch)), Ogre::Radian(MTOF(vroll)));
  Ogre::Quaternion quatResult = euler.toQuaternion();
 
  // Send result to Scol VM.
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM((quatResult.x)));
  MMstore(m, tuple, 1, FTOM((quatResult.y)));
  MMstore(m, tuple, 2, FTOM((quatResult.z)));
  MMstore(m, tuple, 3, FTOM((quatResult.w)));
  MMset(m, 0, PTOM(tuple));
  return 0;
}
 
int SO3MathsQuatToEulerDegreePYR(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerDegreePYR\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  Ogre::Euler euler(quat);
 
  Ogre::Radian xrad = euler.getPitch();
  Ogre::Radian yrad = euler.getYaw();
  Ogre::Radian zrad = euler.getRoll();
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueDegrees()));
  MMstore(m, tuple, 1, FTOM(yrad.valueDegrees()));
  MMstore(m, tuple, 2, FTOM(zrad.valueDegrees()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
int SO3MathsQuatToEulerPYR(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsQuatToEulerPYR\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int x = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int z = MMfetch(m, q, 2);
  int w = MMfetch(m, q, 3);
 
  if ((x == NIL) || (y == NIL) || (z == NIL) || (w == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Quaternion quat = Ogre::Quaternion(MTOF(w), MTOF(x), MTOF(y), MTOF(z));
  Ogre::Euler euler(quat);
 
  Ogre::Radian xrad = euler.getPitch();
  Ogre::Radian yrad = euler.getYaw();
  Ogre::Radian zrad = euler.getRoll();
 
  int tuple = MMmalloc(m, 3, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(xrad.valueRadians()));
  MMstore(m, tuple, 1, FTOM(yrad.valueRadians()));
  MMstore(m, tuple, 2, FTOM(zrad.valueRadians()));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
int SO3MathsEulerPYRToQuat(mmachine m)
{
#ifdef SO3_DEBUG
  MMechostr(MSKDEBUG, "SO3MathsEulerPYRToQuat\n");
#endif
 
  int q = MTOP(MMget(m, 0));
  if (q == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  int p = MMfetch(m, q, 0);
  int y = MMfetch(m, q, 1);
  int r = MMfetch(m, q, 2);
 
  if ((p == NIL) || (y == NIL) || (r == NIL))
  {
    MMset(m, 0, NIL);
    return 0;
  }
 
  Ogre::Euler euler(MTOF(y), MTOF(p), MTOF(r));
  Ogre::Quaternion quat = euler.toQuaternion();
 
  int tuple = MMmalloc(m, 4, TYPETAB);
  if (tuple == NIL)
  {
    MMset(m, 0, NIL);
    return MERRMEM;
  }
  MMstore(m, tuple, 0, FTOM(quat.x));
  MMstore(m, tuple, 1, FTOM(quat.y));
  MMstore(m, tuple, 2, FTOM(quat.z));
  MMstore(m, tuple, 3, FTOM(quat.w));
  MMset(m, 0, PTOM(tuple));
 
  return 0;
}
 
 
NativeDefinition natSO3Maths[] = {
  { "SO3MathsQuatToEulerXYZ",       1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerXYZ },
  { "SO3MathsQuatToEulerXZY",       1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerXZY },
  { "SO3MathsQuatToEulerYXZ",       1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerYXZ },
  { "SO3MathsQuatToEulerYZX",       1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerYZX },
  { "SO3MathsQuatToEulerZXY",       1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerZXY },
  { "SO3MathsQuatToEulerZYX",       1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerZYX },
  { "SO3MathsQuatToEulerDegreeXYZ", 1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerDegreeXYZ },
  { "SO3MathsQuatToEulerDegreeXZY", 1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerDegreeXZY },
  { "SO3MathsQuatToEulerDegreeYXZ", 1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerDegreeYXZ },
  { "SO3MathsQuatToEulerDegreeYZX", 1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerDegreeYZX },
  { "SO3MathsQuatToEulerDegreeZXY", 1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerDegreeZXY },
  { "SO3MathsQuatToEulerDegreeZYX", 1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatToEulerDegreeZYX },
  { "SO3MathsEulerXYZToQuat",       1,  "fun [[F F F]] [F F F F]",                                                                       SO3MathsEulerXYZToQuat },
  { "SO3MathsEulerXZYToQuat",       1,  "fun [[F F F]] [F F F F]",                                                                       SO3MathsEulerXZYToQuat },
  { "SO3MathsEulerYXZToQuat",       1,  "fun [[F F F]] [F F F F]",                                                                       SO3MathsEulerYXZToQuat },
  { "SO3MathsEulerYZXToQuat",       1,  "fun [[F F F]] [F F F F]",                                                                       SO3MathsEulerYZXToQuat },
  { "SO3MathsEulerZYXToQuat",       1,  "fun [[F F F]] [F F F F]",                                                                       SO3MathsEulerZYXToQuat },
  { "SO3MathsEulerZXYToQuat",       1,  "fun [[F F F]] [F F F F]",                                                                       SO3MathsEulerZXYToQuat },
  { "SO3MathsQuatDiff",             2,  "fun [[F F F F] [F F F F]] [F F F F]",                   SO3MathsQuatDiff },
  { "SO3MathsQuatSubstract",        2,  "fun [[F F F F] [F F F F]] [F F F F]",                   SO3MathsQuatSubstract },
  { "SO3MathsQuatAdd",              2,  "fun [[F F F F] [F F F F]] [F F F F]",                   SO3MathsQuatAdd },
  { "SO3MathsDegreeToRadian",       1,  "fun [F] F",                                                                                                                             SO3MathsDegreeToRadian },
  { "SO3MathsRadianToDegree",       1,  "fun [F] F",                                                                                                                             SO3MathsRadianToDegree },
  { "SO3MathsQuatGetRoll",          2,  "fun [[F F F F] I] F",                                                                                   SO3MathsQuatGetRoll },
  { "SO3MathsQuatGetYaw",           2,  "fun [[F F F F] I] F",                                                                                   SO3MathsQuatGetYaw },
  { "SO3MathsQuatGetPitch",         2,  "fun [[F F F F] I] F",                                                                                   SO3MathsQuatGetPitch },
  { "SO3MathsQuatGetDegreeRoll",    2,  "fun [[F F F F] I] F",                                                                                   SO3MathsQuatGetDegreeRoll },
  { "SO3MathsQuatGetDegreeYaw",     2,  "fun [[F F F F] I] F",                                                                                   SO3MathsQuatGetDegreeYaw },
  { "SO3MathsQuatGetDegreePitch",   2,  "fun [[F F F F] I] F",                                                                                   SO3MathsQuatGetDegreePitch },
  { "SO3MathsQuatToAxes",           1,  "fun [[F F F F]] [[F F F] [F F F] [F F F]]", SO3MathsQuatToAxes },
  { "SO3MathsQuatFromAxes",         3,  "fun [[F F F] [F F F] [F F F]] [F F F F]",       SO3MathsQuatFromAxes },
  { "SO3MathsQuatGetXaxis",         1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatGetXaxis },
  { "SO3MathsQuatGetYaxis",         1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatGetYaxis },
  { "SO3MathsQuatGetZaxis",         1,  "fun [[F F F F]] [F F F]",                                                                       SO3MathsQuatGetZaxis },
  { "SO3MathsQuatInterpolate",      4,  "fun [[F F F F] [F F F F] F I] [F F F F]",       SO3MathsQuatInterpolate },
  { "SO3MathsQuatGetDirection",     2,  "fun [[F F F F] [F F F]] [F F F]",                                       SO3MathsQuatGetDirection },
  { "SO3MathsQuatFromAngle",        1,  "fun [[F F F]] [F F F F]",                   SO3MathsQuatFromAngle },
  { "SO3MathsQuatFromDegreeAngle",  1,  "fun [[F F F]] [F F F F]",                   SO3MathsQuatFromDegreeAngle },
  { "SO3MathsQuatFromAngleAxis",    2,  "fun [F [F F F]] [F F F F]",                 SO3MathsQuatFromAngleAxis },
  { "SO3MathsGetRotationTo",        2,  "fun [[F F F] [F F F]] [F F F F]",           SO3MathsGetRotationTo },
  { "SO3MathsGetRotationToAxis",    2,  "fun [[F F F] [F F F]] [F F F F]",           SO3MathsGetRotationToAxis },
  { "SO3MathsClampValue",           3,  "fun [F F F] F",                                                                                                                 SO3MathsClampValue },
  { "SO3MathsVectorDotProduct",     2,  "fun [[F F F] [F F F]] F",                   SO3MathsVectorDotProduct },
  { "SO3MathsQuatRotate",           2,  "fun [[F F F F] [F F F]] [F F F F]",                       SO3MathsQuatRotate },
  { "SO3MathsQuatToEulerDegreePYR", 1,  "fun [[F F F F]] [F F F]",                   SO3MathsQuatToEulerDegreePYR },
  { "SO3MathsQuatToEulerPYR",       1,  "fun [[F F F F]] [F F F]",                   SO3MathsQuatToEulerPYR },
  { "SO3MathsEulerPYRToQuat",       1,  "fun [[F F F]] [F F F F]",                   SO3MathsEulerPYRToQuat },
  { "SO3MathsQuatIsEqual",          2,  "fun [[F F F F] [F F F F]] I",                     SO3MathsQuatIsEqual }
};
 
 
int SCOLloadMaths(mmachine m, cbmachine w)
{
  return PKhardpak2(m, "SO3Maths.pkg", sizeof(natSO3Maths) / sizeof(natSO3Maths[0]), natSO3Maths);
}
 
 
int SCOLfreeMaths()
{
  return 0;
}