imatheuler.h

对gif

    extract(q.toMatrix33());
}

template<class T>
void Euler<T>::extract(const Matrix33<T> &M)
{
    int i,j,k;
    angleOrder(i,j,k);

    if (_initialRepeated)
    {
	//
	// Extract the first angle, x.
	// 

	x = Math<T>::atan2 (M[j][i], M[k][i]);

	//
	// Remove the x rotation from M, so that the remaining
	// rotation, N, is only around two axes, and gimbal lock
	// cannot occur.
	//

	Vec3<T> r (0, 0, 0);
	r[i] = (_parityEven? -x: x);

	Matrix44<T> N;
	N.rotate (r);

	N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
			     M[1][0], M[1][1], M[1][2], 0,
			     M[2][0], M[2][1], M[2][2], 0,
			     0,       0,       0,       1);
	//
	// Extract the other two angles, y and z, from N.
	//

	T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
	y = Math<T>::atan2 (sy, N[i][i]);
	z = Math<T>::atan2 (N[j][k], N[j][j]);
    }
    else
    {
	//
	// Extract the first angle, x.
	// 

	x = Math<T>::atan2 (M[j][k], M[k][k]);

	//
	// Remove the x rotation from M, so that the remaining
	// rotation, N, is only around two axes, and gimbal lock
	// cannot occur.
	//

	Vec3<T> r (0, 0, 0);
	r[i] = (_parityEven? -x: x);

	Matrix44<T> N;
	N.rotate (r);

	N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
			     M[1][0], M[1][1], M[1][2], 0,
			     M[2][0], M[2][1], M[2][2], 0,
			     0,       0,       0,       1);
	//
	// Extract the other two angles, y and z, from N.
	//

	T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
	y = Math<T>::atan2 (-N[i][k], cy);
	z = Math<T>::atan2 (-N[j][i], N[j][j]);
    }

    if (!_parityEven)
	*this *= -1;

    if (!_frameStatic)
    {
	T t = x;
	x = z;
	z = t;
    }
}

template<class T>
void Euler<T>::extract(const Matrix44<T> &M)
{
    int i,j,k;
    angleOrder(i,j,k);

    if (_initialRepeated)
    {
	//
	// Extract the first angle, x.
	// 

	x = Math<T>::atan2 (M[j][i], M[k][i]);

	//
	// Remove the x rotation from M, so that the remaining
	// rotation, N, is only around two axes, and gimbal lock
	// cannot occur.
	//

	Vec3<T> r (0, 0, 0);
	r[i] = (_parityEven? -x: x);

	Matrix44<T> N;
	N.rotate (r);
	N = N * M;

	//
	// Extract the other two angles, y and z, from N.
	//

	T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
	y = Math<T>::atan2 (sy, N[i][i]);
	z = Math<T>::atan2 (N[j][k], N[j][j]);
    }
    else
    {
	//
	// Extract the first angle, x.
	// 

	x = Math<T>::atan2 (M[j][k], M[k][k]);

	//
	// Remove the x rotation from M, so that the remaining
	// rotation, N, is only around two axes, and gimbal lock
	// cannot occur.
	//

	Vec3<T> r (0, 0, 0);
	r[i] = (_parityEven? -x: x);

	Matrix44<T> N;
	N.rotate (r);
	N = N * M;

	//
	// Extract the other two angles, y and z, from N.
	//

	T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
	y = Math<T>::atan2 (-N[i][k], cy);
	z = Math<T>::atan2 (-N[j][i], N[j][j]);
    }

    if (!_parityEven)
	*this *= -1;

    if (!_frameStatic)
    {
	T t = x;
	x = z;
	z = t;
    }
}

template<class T>
Matrix33<T> Euler<T>::toMatrix33() const
{
    int i,j,k;
    angleOrder(i,j,k);

    Vec3<T> angles;

    if ( _frameStatic ) angles = (*this);
    else angles = Vec3<T>(z,y,x);

    if ( !_parityEven ) angles *= -1.0;

    T ci = Math<T>::cos(angles.x);
    T cj = Math<T>::cos(angles.y);
    T ch = Math<T>::cos(angles.z);
    T si = Math<T>::sin(angles.x);
    T sj = Math<T>::sin(angles.y);
    T sh = Math<T>::sin(angles.z);

    T cc = ci*ch;
    T cs = ci*sh;
    T sc = si*ch;
    T ss = si*sh;

    Matrix33<T> M;

    if ( _initialRepeated )
    {
	M[i][i] = cj;	  M[j][i] =  sj*si;    M[k][i] =  sj*ci;
	M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
	M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
    }
    else
    {
	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
	M[i][k] = -sj;	 M[j][k] = cj*si;    M[k][k] = cj*ci;
    }

    return M;
}

template<class T>
Matrix44<T> Euler<T>::toMatrix44() const
{
    int i,j,k;
    angleOrder(i,j,k);

    Vec3<T> angles;

    if ( _frameStatic ) angles = (*this);
    else angles = Vec3<T>(z,y,x);

    if ( !_parityEven ) angles *= -1.0;

    T ci = Math<T>::cos(angles.x);
    T cj = Math<T>::cos(angles.y);
    T ch = Math<T>::cos(angles.z);
    T si = Math<T>::sin(angles.x);
    T sj = Math<T>::sin(angles.y);
    T sh = Math<T>::sin(angles.z);

    T cc = ci*ch;
    T cs = ci*sh;
    T sc = si*ch;
    T ss = si*sh;

    Matrix44<T> M;

    if ( _initialRepeated )
    {
	M[i][i] = cj;	  M[j][i] =  sj*si;    M[k][i] =  sj*ci;
	M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
	M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
    }
    else
    {
	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
	M[i][k] = -sj;	 M[j][k] = cj*si;    M[k][k] = cj*ci;
    }

    return M;
}

template<class T>
Quat<T> Euler<T>::toQuat() const
{
    Vec3<T> angles;
    int i,j,k;
    angleOrder(i,j,k);

    if ( _frameStatic ) angles = (*this);
    else angles = Vec3<T>(z,y,x);

    if ( !_parityEven ) angles.y = -angles.y;

    T ti = angles.x*0.5;
    T tj = angles.y*0.5;
    T th = angles.z*0.5;
    T ci = Math<T>::cos(ti);
    T cj = Math<T>::cos(tj);
    T ch = Math<T>::cos(th);
    T si = Math<T>::sin(ti);
    T sj = Math<T>::sin(tj);
    T sh = Math<T>::sin(th);
    T cc = ci*ch;
    T cs = ci*sh;
    T sc = si*ch;
    T ss = si*sh;

    T parity = _parityEven ? 1.0 : -1.0;

    Quat<T> q;
    Vec3<T> a;

    if ( _initialRepeated )
    {
	a[i]	= cj*(cs + sc);
	a[j]	= sj*(cc + ss) * parity,
	a[k]	= sj*(cs - sc);
	q.r	= cj*(cc - ss);
    }
    else
    {
	a[i]	= cj*sc - sj*cs,
	a[j]	= (cj*ss + sj*cc) * parity,
	a[k]	= cj*cs - sj*sc;
	q.r	= cj*cc + sj*ss;
    }

    q.v = a;

    return q;
}

template<class T>
inline bool
Euler<T>::legal(typename Euler<T>::Order order)
{
    return (order & ~Legal) ? false : true;
}

template<class T>
typename Euler<T>::Order
Euler<T>::order() const
{
    int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));

    if (_parityEven)	  foo |= 0x0100;
    if (_initialRepeated) foo |= 0x0010;
    if (_frameStatic)	  foo++;

    return (Order)foo;
}

template<class T>
inline void Euler<T>::setOrder(typename Euler<T>::Order p)
{
    set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X),	// initial axis
	 !(p & 0x1),	    			// static?
	 !!(p & 0x100),				// permutation even?
	 !!(p & 0x10));				// initial repeats?
}

template<class T>
void Euler<T>::set(typename Euler<T>::Axis axis,
		   bool relative,
		   bool parityEven,
		   bool firstRepeats)
{
    _initialAxis	= axis;
    _frameStatic	= !relative;
    _parityEven		= parityEven;
    _initialRepeated	= firstRepeats;
}

template<class T>
const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
{
    x = euler.x;
    y = euler.y;
    z = euler.z;
    _initialAxis = euler._initialAxis;
    _frameStatic = euler._frameStatic;
    _parityEven	 = euler._parityEven;
    _initialRepeated = euler._initialRepeated;
    return *this;
}

template<class T>
const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
{
    x = v.x;
    y = v.y;
    z = v.z;
    return *this;
}

template<class T>
std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
{
    char a[3] = { 'X', 'Y', 'Z' };

    const char* r = euler.frameStatic() ? "" : "r";
    int i,j,k;
    euler.angleOrder(i,j,k);

    if ( euler.initialRepeated() ) k = i;

    return o << "("
	     << euler.x << " "
	     << euler.y << " "
	     << euler.z << " "
	     << a[i] << a[j] << a[k] << r << ")";
}

template <class T>
float
Euler<T>::angleMod (T angle)
{
    angle = fmod(T (angle), T (2 * M_PI));

    if (angle < -M_PI)	angle += 2 * M_PI;
    if (angle > +M_PI)	angle -= 2 * M_PI;

    return angle;
}

template <class T>
void
Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
{
    Vec3<T> d  = xyzRot - targetXyzRot;
    xyzRot[0]  = targetXyzRot[0] + angleMod(d[0]);
    xyzRot[1]  = targetXyzRot[1] + angleMod(d[1]);
    xyzRot[2]  = targetXyzRot[2] + angleMod(d[2]);
}

template <class T>
void
Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
			   Order order)
{
    int i,j,k;
    Euler<T> e (0,0,0, order);
    e.angleOrder(i,j,k);

    simpleXYZRotation(xyzRot, targetXyzRot);

    Vec3<T> otherXyzRot;
    otherXyzRot[i] = M_PI+xyzRot[i];
    otherXyzRot[j] = M_PI-xyzRot[j];
    otherXyzRot[k] = M_PI+xyzRot[k];

    simpleXYZRotation(otherXyzRot, targetXyzRot);
	    
    Vec3<T> d  = xyzRot - targetXyzRot;
    Vec3<T> od = otherXyzRot - targetXyzRot;
    T dMag     = d.dot(d);
    T odMag    = od.dot(od);

    if (odMag < dMag)
    {
	xyzRot = otherXyzRot;
    }
}

template <class T>
void
Euler<T>::makeNear (const Euler<T> &target)
{
    Vec3<T> xyzRot    = toXYZVector();
    Euler<T> targetSameOrder = Euler<T>(target, order());
    Vec3<T> targetXyz = targetSameOrder.toXYZVector();

    nearestRotation(xyzRot, targetXyz, order());

    setXYZVector(xyzRot);
}

#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
#pragma warning(default:4244)
#endif

} // namespace Imath


#endif