// -*-c++-*-
#ifndef IG_STAT_COORDSYS_H
#define IG_STAT_COORDSYS_H
// $Id: stat_coordsys.h,v 1.1.2.6 2005/05/30 09:14:25 hkuiper Exp $
// CwMtx matrix and vector math library
// Copyright (C) 1999-2001 Harry Kuiper
// Copyright (C) 2000 Will DeVore (template conversion)
// This library 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 library 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 library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
// USA
#ifndef IG_STAT_SMATRIX_H
#include "stat_smatrix.h"
#endif
#ifndef IG_STAT_QUATERNION_H
#include "stat_quatern.h"
#endif
#ifndef IG_STAT_SVECTOR_H
#include "stat_svector.h"
#endif
// CWTSSquareMatrix utilities for use in coordinate systems
namespace CwMtx
{
// NOTE: These template functions only work with floating point
// template arguments due to the use of sin(3), cos(3), tan(3), etc.
// PLEASE, READ THIS!!!
// Function smatFromEuler321(sclrAbout3, sclrAbout2, sclrAbout1)
// returns a transformation matrix which transforms coordinates from
// the reference axis system to coordinates in an axis system with
// Euler angles: sclrAbout3, sclrAbout2, sclrAbout1 relative to that
// reference axis system DO NOT CHANGE THIS DEFINITION!!!
// rotation about axis 3 (yaw angle), axis 2 (pitch angle) axis 1
// (roll angle)
template <class T>
CWTSSquareMatrix<3, T>
smatFromEuler321Angles(T sclrAbout3,
T sclrAbout2,
T sclrAbout1)
{
CWTSSquareMatrix<3, T> smat;
// to reduce the number of calls to CPU expensive sin(..) and
// cos(..) functions
T sin3 = sin(sclrAbout3);
T sin2 = sin(sclrAbout2);
T sin1 = sin(sclrAbout1);
T cos3 = cos(sclrAbout3);
T cos2 = cos(sclrAbout2);
T cos1 = cos(sclrAbout1);
// first row
smat[0][0] = cos3*cos2;
smat[0][1] = sin3*cos2;
smat[0][2] = -sin2;
// second row
smat[1][0] = -sin3*cos1 + cos3*sin2*sin1;
smat[1][1] = cos3*cos1 + sin3*sin2*sin1;
smat[1][2] = cos2*sin1;
// third row
smat[2][0] = sin3*sin1 + cos3*sin2*cos1;
smat[2][1] = -cos3*sin1 + sin3*sin2*cos1;
smat[2][2] = cos2*cos1;
return smat;
}
// Next three functions calculate Euler angles from a transformation matrix
// WARNING!!! They suffer from the ubiquitos singularities at 0, pi, 2*pi,
// etc.
// yaw angle, rotation angle about Z-axis
template <class T>
T
euler321Angle3FromSmat(const CWTSSquareMatrix<3, T> &smat)
{
return atan2(smat[0][1], smat[0][0]);
}
// pitch angle, rotation angle about Y-axis
template <class T>
T
euler321Angle2FromSmat(const CWTSSquareMatrix<3, T> &smat)
{
return asin(-smat[0][2]);
}
// roll angle, rotation angle about X-axis
template <class T>
T
euler321Angle1FromSmat(const CWTSSquareMatrix<3, T> &smat)
{
return atan2(smat[1][2], smat[2][2]);
}
// CWTSQuaternion utilities for use in coordinate systems
// PLEASE, READ THIS!!!
// next three functions calculate Euler angles from a quaternion
// that represents a rigid body's attitude WARNING!!! They do suffer
// from the ubiquitos singularities around 0, pi and 2*pi.
// yaw angle, rotation angle about Z-axis
template <class T>
T
euler321Angle3FromQtn(const CWTSQuaternion<T> &qtn)
{
// to reduce the number of calls to the subscript operator
T qtn0 = qtn[0];
T qtn1 = qtn[1];
T qtn2 = qtn[2];
T qtn3 = qtn[3];
// NOTE: Speed tip from Jeffrey T Duncan: use the identity: 1 =
// q0^2 + q1^2 + q2^2 + q3^2 which simplifies: q0^2 - q1^2 -
// q2^2 + q3^2 down to: 2*(q0^2 + q3^2) - 1
return atan2(2*(qtn0*qtn1 + qtn2*qtn3),
2*(qtn0*qtn0 + qtn3*qtn3) - 1);
}
// pitch angle, rotation angle about Y-axis
template <class T>
T
euler321Angle2FromQtn(const CWTSQuaternion<T> &qtn)
{
return asin(-2*(qtn[0]*qtn[2] - qtn[1]*qtn[3]));
}
// roll angle, rotation angle about X-axis
template <class T>
T
euler321Angle1FromQtn(const CWTSQuaternion<T> &qtn)
{
// to reduce the number of calls to the subscript operator
T qtn0 = qtn[0];
T qtn1 = qtn[1];
T qtn2 = qtn[2];
T qtn3 = qtn[3];
return atan2(2*(qtn1*qtn2 + qtn0*qtn3),
2*(qtn2*qtn2 + qtn3*qtn3) - 1);
}
// Function QtnFromEulerAxisAndAngle returns a quaternion
// representing a rigid body's attitude from its Euler axis and
// angle. NOTE: Argument vector svecEulerAxis MUST be a unit vector.
template <class T>
CWTSQuaternion<T>
qtnFromEulerAxisAndAngle(const CWTSSpaceVector<T> &svecEulerAxis,
const T &sclrEulerAngle)
{
return CWTSQuaternion<T>(1, svecEulerAxis, 0.5*sclrEulerAngle);
}
// returns Euler axis
template <class T>
CWTSSpaceVector<T>
eulerAxisFromQtn(const CWTSQuaternion<T> &qtn)
{
T sclrTmp = sqrt(1 - qtn[3]*qtn[3]);
return CWTSSpaceVector<T>(qtn[0]/sclrTmp,
qtn[1]/sclrTmp,
qtn[2]/sclrTmp);
}
// returns Euler angle
template <class T>
T
eulerAngleFromQtn(const CWTSQuaternion<T> &qtn)
{
return 2*acos(qtn[3]);
}
// Function QtnFromEuler321 returns a quaternion representing a
// rigid body's attitude. The quaternion elements correspond to the
// Euler symmetric parameters of the body. The body's attitude must
// be entered in Euler angle representation with rotation order
// 3-2-1, i.e. first about Z-axis next about Y-axis and finally
// about X-axis.
// rotation about axis 3 (yaw angle), axis 2 (pitch angle) axis 1
// (roll angle)
template <class T>
CWTSQuaternion<T>
qtnFromEuler321Angles(T sclrAbout3,
T sclrAbout2,
T sclrAbout1)
{
return
CWTSQuaternion<T>(1, CWTSSpaceVector<T>(0, 0, 1), 0.5*sclrAbout3)
*CWTSQuaternion<T>(1, CWTSSpaceVector<T>(0, 1, 0), 0.5*sclrAbout2)
*CWTSQuaternion<T>(1, CWTSSpaceVector<T>(1, 0, 0), 0.5*sclrAbout1);
}
// SmatFromQtn returns the transformation matrix corresponding to a
// quaternion
template <class T>
CWTSSquareMatrix<3, T>
smatFromQtn(const CWTSQuaternion<T> &qtn)
{
CWTSSquareMatrix<3, T> smat;
// to reduce the number of calls to the subscript operator
T qtn0 = qtn[0];
T qtn1 = qtn[1];
T qtn2 = qtn[2];
T qtn3 = qtn[3];
smat[0][0] = 1 - 2*(qtn1*qtn1 + qtn2*qtn2);
smat[0][1] = 2*(qtn0*qtn1 + qtn2*qtn3);
smat[0][2] = 2*(qtn0*qtn2 - qtn1*qtn3);
smat[1][0] = 2*(qtn0*qtn1 - qtn2*qtn3);
smat[1][1] = 1 - 2*(qtn0*qtn0 + qtn2*qtn2);
smat[1][2] = 2*(qtn1*qtn2 + qtn0*qtn3);
smat[2][0] = 2*(qtn0*qtn2 + qtn1*qtn3);
smat[2][1] = 2*(qtn1*qtn2 - qtn0*qtn3);
smat[2][2] = 1 - 2*(qtn0*qtn0 + qtn1*qtn1);
return smat;
}
// QtnFromSmat returns the quaternion corresponding to a
// transformation matrix
template <class T>
CWTSQuaternion<T>
qtnFromSmat(const CWTSSquareMatrix<3, T> &smat)
{
// Check trace to prevent loss of accuracy
T sclrTmp = tr(smat);
CWTSQuaternion<T> qtn;
if(sclrTmp > 0)
{
// No trouble, we can use standard expression
sclrTmp = 0.5*sqrt(1 + sclrTmp);
qtn[0] = 0.25*(smat[1][2] - smat[2][1])/sclrTmp;
qtn[1] = 0.25*(smat[2][0] - smat[0][2])/sclrTmp;
qtn[2] = 0.25*(smat[0][1] - smat[1][0])/sclrTmp;
qtn[3] = sclrTmp;
}
else
{
// We could be in trouble, so we have to take
// precautions. NOTE: Parts of this piece of code were taken
// from the example in the article "Rotating Objects Using
// Quaternions" in Game Developer Magazine written by Nick
// Bobick, july 3, 1998, vol. 2, issue 26.
int i = 0;
if (smat[1][1] > smat[0][0]) i = 1;
if (smat[2][2] > smat[i][i]) i = 2;
switch (i)
{
case 0:
sclrTmp = 0.5*sqrt(1 + smat[0][0] - smat[1][1] - smat[2][2]);
qtn[0] = sclrTmp;
qtn[1] = 0.25*(smat[0][1] + smat[1][0])/sclrTmp;
qtn[2] = 0.25*(smat[0][2] + smat[2][0])/sclrTmp;
qtn[3] = 0.25*(smat[1][2] - smat[2][1])/sclrTmp;
break;
case 1:
sclrTmp = 0.5*sqrt(1 - smat[0][0] + smat[1][1] - smat[2][2]);
qtn[0] = 0.25*(smat[1][0] + smat[0][1])/sclrTmp;
qtn[1] = sclrTmp;
qtn[2] = 0.25*(smat[1][2] + smat[2][1])/sclrTmp;
qtn[3] = 0.25*(smat[2][0] - smat[0][2])/sclrTmp;
break;
case 2:
sclrTmp = 0.5*sqrt(1 - smat[0][0] - smat[1][1] + smat[2][2]);
qtn[0] = 0.25*(smat[2][0] + smat[0][2])/sclrTmp;
qtn[1] = 0.25*(smat[2][1] + smat[1][2])/sclrTmp;
qtn[2] = sclrTmp;
qtn[3] = 0.25*(smat[0][1] - smat[1][0])/sclrTmp;
break;
}
}
return qtn;
}
// NOTE: This function duplicates eulerAxisFromQtn(qtn) it only has
// a different return type.
template <class T>
CWTSVector<3, T>
axisAngleFromQtn( const CWTSQuaternion<T> &qtn )
{
return eulerAxisFromQtn(qtn);
}
// NOTE: This function duplicates qtnFromEulerAxisAndAngle(..) it
// only has a different function profile.
template <class T>
CWTSQuaternion<T>
qtnFromAxisAndAngle(const CWTSVector<3, T> &vAxis,
const T sAngle)
{
return qtnFromEulerAxisAndAngle(vAxis, sAngle);
}
template <class T>
CWTSSquareMatrix<3, T>
changeOfBasis(CWTSSpaceVector< CWTSSpaceVector<T> >&from,
CWTSSpaceVector< CWTSSpaceVector<T> >&to)
{
CWTSSquareMatrix<3, T> A;
// This is a "change of basis" from the "from" frame to the "to"
// frame. the "to" frame is typically the "Standard basis"
// frame. The "from" is a frame that represents the current
// orientation of the object in the shape of an orthonormal
// tripod.
enum { x , y , z };
// _X,_Y,_Z is typically the standard basis frame.
// | x^_X , y^_X , z^_X |
// | x^_Y , y^_Y , z^_Y |
// | x^_Z , y^_Z , z^_Z |
A[0][0] = from[x]*to[x];
A[0][1] = from[y]*to[x];
A[0][2] = from[z]*to[x];
A[1][0] = from[x]*to[y];
A[1][1] = from[y]*to[y];
A[1][2] = from[z]*to[y];
A[2][0] = from[x]*to[z];
A[2][1] = from[y]*to[z];
A[2][2] = from[z]*to[z];
// This is simply the transpose done manually which would save
// and inverse call.
// | x ^ _X , x ^ _Y , x ^ _Z |
// | y ^ _X , y ^ _Y , y ^ _Z |
// | z ^ _X , z ^ _Y , z ^ _Z |
// And finally we return the matrix that takes the "from" frame
// to the "to"/"Standard basis" frame.
return A;
}
}
#endif // IG_STAT_COORDSYS_H