|
Packit |
8dc392 |
///////////////////////////////////////////////////////////////////////////
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Copyright (c) 2002-2012, Industrial Light & Magic, a division of Lucas
|
|
Packit |
8dc392 |
// Digital Ltd. LLC
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// All rights reserved.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Redistribution and use in source and binary forms, with or without
|
|
Packit |
8dc392 |
// modification, are permitted provided that the following conditions are
|
|
Packit |
8dc392 |
// met:
|
|
Packit |
8dc392 |
// * Redistributions of source code must retain the above copyright
|
|
Packit |
8dc392 |
// notice, this list of conditions and the following disclaimer.
|
|
Packit |
8dc392 |
// * Redistributions in binary form must reproduce the above
|
|
Packit |
8dc392 |
// copyright notice, this list of conditions and the following disclaimer
|
|
Packit |
8dc392 |
// in the documentation and/or other materials provided with the
|
|
Packit |
8dc392 |
// distribution.
|
|
Packit |
8dc392 |
// * Neither the name of Industrial Light & Magic nor the names of
|
|
Packit |
8dc392 |
// its contributors may be used to endorse or promote products derived
|
|
Packit |
8dc392 |
// from this software without specific prior written permission.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
Packit |
8dc392 |
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
Packit |
8dc392 |
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
|
Packit |
8dc392 |
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
|
Packit |
8dc392 |
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
|
Packit |
8dc392 |
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
|
Packit |
8dc392 |
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
|
Packit |
8dc392 |
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
|
Packit |
8dc392 |
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
Packit |
8dc392 |
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
Packit |
8dc392 |
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
///////////////////////////////////////////////////////////////////////////
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
#ifndef INCLUDED_IMATHEULER_H
|
|
Packit |
8dc392 |
#define INCLUDED_IMATHEULER_H
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//----------------------------------------------------------------------
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// template class Euler<T>
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// This class represents euler angle orientations. The class
|
|
Packit |
8dc392 |
// inherits from Vec3 to it can be freely cast. The additional
|
|
Packit |
8dc392 |
// information is the euler priorities rep. This class is
|
|
Packit |
8dc392 |
// essentially a rip off of Ken Shoemake's GemsIV code. It has
|
|
Packit |
8dc392 |
// been modified minimally to make it more understandable, but
|
|
Packit |
8dc392 |
// hardly enough to make it easy to grok completely.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// There are 24 possible combonations of Euler angle
|
|
Packit |
8dc392 |
// representations of which 12 are common in CG and you will
|
|
Packit |
8dc392 |
// probably only use 6 of these which in this scheme are the
|
|
Packit |
8dc392 |
// non-relative-non-repeating types.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// The representations can be partitioned according to two
|
|
Packit |
8dc392 |
// criteria:
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// 1) Are the angles measured relative to a set of fixed axis
|
|
Packit |
8dc392 |
// or relative to each other (the latter being what happens
|
|
Packit |
8dc392 |
// when rotation matrices are multiplied together and is
|
|
Packit |
8dc392 |
// almost ubiquitous in the cg community)
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// 2) Is one of the rotations repeated (ala XYX rotation)
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// When you construct a given representation from scratch you
|
|
Packit |
8dc392 |
// must order the angles according to their priorities. So, the
|
|
Packit |
8dc392 |
// easiest is a softimage or aerospace (yaw/pitch/roll) ordering
|
|
Packit |
8dc392 |
// of ZYX.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// float x_rot = 1;
|
|
Packit |
8dc392 |
// float y_rot = 2;
|
|
Packit |
8dc392 |
// float z_rot = 3;
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX);
|
|
Packit |
8dc392 |
// -or-
|
|
Packit |
8dc392 |
// Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX );
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// If instead, the order was YXZ for instance you would have to
|
|
Packit |
8dc392 |
// do this:
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// float x_rot = 1;
|
|
Packit |
8dc392 |
// float y_rot = 2;
|
|
Packit |
8dc392 |
// float z_rot = 3;
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
|
|
Packit |
8dc392 |
// -or-
|
|
Packit |
8dc392 |
// Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ );
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Notice how the order you put the angles into the three slots
|
|
Packit |
8dc392 |
// should correspond to the enum (YXZ) ordering. The input angle
|
|
Packit |
8dc392 |
// vector is called the "ijk" vector -- not an "xyz" vector. The
|
|
Packit |
8dc392 |
// ijk vector order is the same as the enum. If you treat the
|
|
Packit |
8dc392 |
// Euler<> as a Vec<> (which it inherts from) you will find the
|
|
Packit |
8dc392 |
// angles are ordered in the same way, i.e.:
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// V3f v = angles;
|
|
Packit |
8dc392 |
// // v.x == y_rot, v.y == x_rot, v.z == z_rot
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// If you just want the x, y, and z angles stored in a vector in
|
|
Packit |
8dc392 |
// that order, you can do this:
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// V3f v = angles.toXYZVector()
|
|
Packit |
8dc392 |
// // v.x == x_rot, v.y == y_rot, v.z == z_rot
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// If you want to set the Euler with an XYZVector use the
|
|
Packit |
8dc392 |
// optional layout argument:
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Eulerf angles(x_rot, y_rot, z_rot,
|
|
Packit |
8dc392 |
// Eulerf::YXZ,
|
|
Packit |
8dc392 |
// Eulerf::XYZLayout);
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// This is the same as:
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Note that this won't do anything intelligent if you have a
|
|
Packit |
8dc392 |
// repeated axis in the euler angles (e.g. XYX)
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// If you need to use the "relative" versions of these, you will
|
|
Packit |
8dc392 |
// need to use the "r" enums.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// The units of the rotation angles are assumed to be radians.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
//----------------------------------------------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
#include "ImathMath.h"
|
|
Packit |
8dc392 |
#include "ImathVec.h"
|
|
Packit |
8dc392 |
#include "ImathQuat.h"
|
|
Packit |
8dc392 |
#include "ImathMatrix.h"
|
|
Packit |
8dc392 |
#include "ImathLimits.h"
|
|
Packit |
8dc392 |
#include "ImathNamespace.h"
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
#include <iostream>
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
IMATH_INTERNAL_NAMESPACE_HEADER_ENTER
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
|
|
Packit |
8dc392 |
// Disable MS VC++ warnings about conversion from double to float
|
|
Packit |
8dc392 |
#pragma warning(disable:4244)
|
|
Packit |
8dc392 |
#endif
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template <class T>
|
|
Packit |
8dc392 |
class Euler : public Vec3<T>
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
public:
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
using Vec3<T>::x;
|
|
Packit |
8dc392 |
using Vec3<T>::y;
|
|
Packit |
8dc392 |
using Vec3<T>::z;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
enum Order
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// All 24 possible orderings
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
XYZ = 0x0101, // "usual" orderings
|
|
Packit |
8dc392 |
XZY = 0x0001,
|
|
Packit |
8dc392 |
YZX = 0x1101,
|
|
Packit |
8dc392 |
YXZ = 0x1001,
|
|
Packit |
8dc392 |
ZXY = 0x2101,
|
|
Packit |
8dc392 |
ZYX = 0x2001,
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
XZX = 0x0011, // first axis repeated
|
|
Packit |
8dc392 |
XYX = 0x0111,
|
|
Packit |
8dc392 |
YXY = 0x1011,
|
|
Packit |
8dc392 |
YZY = 0x1111,
|
|
Packit |
8dc392 |
ZYZ = 0x2011,
|
|
Packit |
8dc392 |
ZXZ = 0x2111,
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
XYZr = 0x2000, // relative orderings -- not common
|
|
Packit |
8dc392 |
XZYr = 0x2100,
|
|
Packit |
8dc392 |
YZXr = 0x1000,
|
|
Packit |
8dc392 |
YXZr = 0x1100,
|
|
Packit |
8dc392 |
ZXYr = 0x0000,
|
|
Packit |
8dc392 |
ZYXr = 0x0100,
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
XZXr = 0x2110, // relative first axis repeated
|
|
Packit |
8dc392 |
XYXr = 0x2010,
|
|
Packit |
8dc392 |
YXYr = 0x1110,
|
|
Packit |
8dc392 |
YZYr = 0x1010,
|
|
Packit |
8dc392 |
ZYZr = 0x0110,
|
|
Packit |
8dc392 |
ZXZr = 0x0010,
|
|
Packit |
8dc392 |
// ||||
|
|
Packit |
8dc392 |
// VVVV
|
|
Packit |
8dc392 |
// Legend: ABCD
|
|
Packit |
8dc392 |
// A -> Initial Axis (0==x, 1==y, 2==z)
|
|
Packit |
8dc392 |
// B -> Parity Even (1==true)
|
|
Packit |
8dc392 |
// C -> Initial Repeated (1==true)
|
|
Packit |
8dc392 |
// D -> Frame Static (1==true)
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Legal = XYZ | XZY | YZX | YXZ | ZXY | ZYX |
|
|
Packit |
8dc392 |
XZX | XYX | YXY | YZY | ZYZ | ZXZ |
|
|
Packit |
8dc392 |
XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr|
|
|
Packit |
8dc392 |
XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr,
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Min = 0x0000,
|
|
Packit |
8dc392 |
Max = 0x2111,
|
|
Packit |
8dc392 |
Default = XYZ
|
|
Packit |
8dc392 |
};
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
enum Axis { X = 0, Y = 1, Z = 2 };
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
enum InputLayout { XYZLayout, IJKLayout };
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//--------------------------------------------------------------------
|
|
Packit |
8dc392 |
// Constructors -- all default to ZYX non-relative ala softimage
|
|
Packit |
8dc392 |
// (where there is no argument to specify it)
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// The Euler-from-matrix constructors assume that the matrix does
|
|
Packit |
8dc392 |
// not include shear or non-uniform scaling, but the constructors
|
|
Packit |
8dc392 |
// do not examine the matrix to verify this assumption. If necessary,
|
|
Packit |
8dc392 |
// you can adjust the matrix by calling the removeScalingAndShear()
|
|
Packit |
8dc392 |
// function, defined in ImathMatrixAlgo.h.
|
|
Packit |
8dc392 |
//--------------------------------------------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Euler();
|
|
Packit |
8dc392 |
Euler(const Euler&);
|
|
Packit |
8dc392 |
Euler(Order p);
|
|
Packit |
8dc392 |
Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
|
|
Packit |
8dc392 |
Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
|
|
Packit |
8dc392 |
Euler(const Euler<T> &euler, Order newp);
|
|
Packit |
8dc392 |
Euler(const Matrix33<T> &, Order o = Default);
|
|
Packit |
8dc392 |
Euler(const Matrix44<T> &, Order o = Default);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//---------------------------------
|
|
Packit |
8dc392 |
// Algebraic functions/ Operators
|
|
Packit |
8dc392 |
//---------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
const Euler<T>& operator= (const Euler<T>&);
|
|
Packit |
8dc392 |
const Euler<T>& operator= (const Vec3<T>&);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//--------------------------------------------------------
|
|
Packit |
8dc392 |
// Set the euler value
|
|
Packit |
8dc392 |
// This does NOT convert the angles, but setXYZVector()
|
|
Packit |
8dc392 |
// does reorder the input vector.
|
|
Packit |
8dc392 |
//--------------------------------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
static bool legal(Order);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
void setXYZVector(const Vec3<T> &);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Order order() const;
|
|
Packit |
8dc392 |
void setOrder(Order);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
void set(Axis initial,
|
|
Packit |
8dc392 |
bool relative,
|
|
Packit |
8dc392 |
bool parityEven,
|
|
Packit |
8dc392 |
bool firstRepeats);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//------------------------------------------------------------
|
|
Packit |
8dc392 |
// Conversions, toXYZVector() reorders the angles so that
|
|
Packit |
8dc392 |
// the X rotation comes first, followed by the Y and Z
|
|
Packit |
8dc392 |
// in cases like XYX ordering, the repeated angle will be
|
|
Packit |
8dc392 |
// in the "z" component
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// The Euler-from-matrix extract() functions assume that the
|
|
Packit |
8dc392 |
// matrix does not include shear or non-uniform scaling, but
|
|
Packit |
8dc392 |
// the extract() functions do not examine the matrix to verify
|
|
Packit |
8dc392 |
// this assumption. If necessary, you can adjust the matrix
|
|
Packit |
8dc392 |
// by calling the removeScalingAndShear() function, defined
|
|
Packit |
8dc392 |
// in ImathMatrixAlgo.h.
|
|
Packit |
8dc392 |
//------------------------------------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
void extract(const Matrix33<T>&);
|
|
Packit |
8dc392 |
void extract(const Matrix44<T>&);
|
|
Packit |
8dc392 |
void extract(const Quat<T>&);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Matrix33<T> toMatrix33() const;
|
|
Packit |
8dc392 |
Matrix44<T> toMatrix44() const;
|
|
Packit |
8dc392 |
Quat<T> toQuat() const;
|
|
Packit |
8dc392 |
Vec3<T> toXYZVector() const;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//---------------------------------------------------
|
|
Packit |
8dc392 |
// Use this function to unpack angles from ijk form
|
|
Packit |
8dc392 |
//---------------------------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
void angleOrder(int &i, int &j, int &k) const;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//---------------------------------------------------
|
|
Packit |
8dc392 |
// Use this function to determine mapping from xyz to ijk
|
|
Packit |
8dc392 |
// - reshuffles the xyz to match the order
|
|
Packit |
8dc392 |
//---------------------------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
void angleMapping(int &i, int &j, int &k) const;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//----------------------------------------------------------------------
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Utility methods for getting continuous rotations. None of these
|
|
Packit |
8dc392 |
// methods change the orientation given by its inputs (or at least
|
|
Packit |
8dc392 |
// that is the intent).
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// angleMod() converts an angle to its equivalent in [-PI, PI]
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// simpleXYZRotation() adjusts xyzRot so that its components differ
|
|
Packit |
8dc392 |
// from targetXyzRot by no more than +-PI
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// nearestRotation() adjusts xyzRot so that its components differ
|
|
Packit |
8dc392 |
// from targetXyzRot by as little as possible.
|
|
Packit |
8dc392 |
// Note that xyz here really means ijk, because
|
|
Packit |
8dc392 |
// the order must be provided.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// makeNear() adjusts "this" Euler so that its components differ
|
|
Packit |
8dc392 |
// from target by as little as possible. This method
|
|
Packit |
8dc392 |
// might not make sense for Eulers with different order
|
|
Packit |
8dc392 |
// and it probably doesn't work for repeated axis and
|
|
Packit |
8dc392 |
// relative orderings (TODO).
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
//-----------------------------------------------------------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
static float angleMod (T angle);
|
|
Packit |
8dc392 |
static void simpleXYZRotation (Vec3<T> &xyzRot,
|
|
Packit |
8dc392 |
const Vec3<T> &targetXyzRot);
|
|
Packit |
8dc392 |
static void nearestRotation (Vec3<T> &xyzRot,
|
|
Packit |
8dc392 |
const Vec3<T> &targetXyzRot,
|
|
Packit |
8dc392 |
Order order = XYZ);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
void makeNear (const Euler<T> &target);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
bool frameStatic() const { return _frameStatic; }
|
|
Packit |
8dc392 |
bool initialRepeated() const { return _initialRepeated; }
|
|
Packit |
8dc392 |
bool parityEven() const { return _parityEven; }
|
|
Packit |
8dc392 |
Axis initialAxis() const { return _initialAxis; }
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
protected:
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
bool _frameStatic : 1; // relative or static rotations
|
|
Packit |
8dc392 |
bool _initialRepeated : 1; // init axis repeated as last
|
|
Packit |
8dc392 |
bool _parityEven : 1; // "parity of axis permutation"
|
|
Packit |
8dc392 |
#if defined _WIN32 || defined _WIN64
|
|
Packit |
8dc392 |
Axis _initialAxis ; // First axis of rotation
|
|
Packit |
8dc392 |
#else
|
|
Packit |
8dc392 |
Axis _initialAxis : 2; // First axis of rotation
|
|
Packit |
8dc392 |
#endif
|
|
Packit |
8dc392 |
};
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//--------------------
|
|
Packit |
8dc392 |
// Convenient typedefs
|
|
Packit |
8dc392 |
//--------------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
typedef Euler<float> Eulerf;
|
|
Packit |
8dc392 |
typedef Euler<double> Eulerd;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//---------------
|
|
Packit |
8dc392 |
// Implementation
|
|
Packit |
8dc392 |
//---------------
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline void
|
|
Packit |
8dc392 |
Euler<T>::angleOrder(int &i, int &j, int &k) const
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
i = _initialAxis;
|
|
Packit |
8dc392 |
j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
|
|
Packit |
8dc392 |
k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline void
|
|
Packit |
8dc392 |
Euler<T>::angleMapping(int &i, int &j, int &k) const
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int m[3];
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
m[_initialAxis] = 0;
|
|
Packit |
8dc392 |
m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
|
|
Packit |
8dc392 |
m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
|
|
Packit |
8dc392 |
i = m[0];
|
|
Packit |
8dc392 |
j = m[1];
|
|
Packit |
8dc392 |
k = m[2];
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline void
|
|
Packit |
8dc392 |
Euler<T>::setXYZVector(const Vec3<T> &v)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
angleMapping(i,j,k);
|
|
Packit |
8dc392 |
(*this)[i] = v.x;
|
|
Packit |
8dc392 |
(*this)[j] = v.y;
|
|
Packit |
8dc392 |
(*this)[k] = v.z;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline Vec3<T>
|
|
Packit |
8dc392 |
Euler<T>::toXYZVector() const
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
angleMapping(i,j,k);
|
|
Packit |
8dc392 |
return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
Euler<T>::Euler() :
|
|
Packit |
8dc392 |
Vec3<T>(0,0,0),
|
|
Packit |
8dc392 |
_frameStatic(true),
|
|
Packit |
8dc392 |
_initialRepeated(false),
|
|
Packit |
8dc392 |
_parityEven(true),
|
|
Packit |
8dc392 |
_initialAxis(X)
|
|
Packit |
8dc392 |
{}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
Euler<T>::Euler(typename Euler<T>::Order p) :
|
|
Packit |
8dc392 |
Vec3<T>(0,0,0),
|
|
Packit |
8dc392 |
_frameStatic(true),
|
|
Packit |
8dc392 |
_initialRepeated(false),
|
|
Packit |
8dc392 |
_parityEven(true),
|
|
Packit |
8dc392 |
_initialAxis(X)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
setOrder(p);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline Euler<T>::Euler( const Vec3<T> &v,
|
|
Packit |
8dc392 |
typename Euler<T>::Order p,
|
|
Packit |
8dc392 |
typename Euler<T>::InputLayout l )
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
setOrder(p);
|
|
Packit |
8dc392 |
if ( l == XYZLayout ) setXYZVector(v);
|
|
Packit |
8dc392 |
else { x = v.x; y = v.y; z = v.z; }
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline Euler<T>::Euler(const Euler<T> &euler)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
operator=(euler);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline Euler<T>::Euler(const Euler<T> &euler,Order p)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
setOrder(p);
|
|
Packit |
8dc392 |
Matrix33<T> M = euler.toMatrix33();
|
|
Packit |
8dc392 |
extract(M);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline Euler<T>::Euler( T xi, T yi, T zi,
|
|
Packit |
8dc392 |
typename Euler<T>::Order p,
|
|
Packit |
8dc392 |
typename Euler<T>::InputLayout l)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
setOrder(p);
|
|
Packit |
8dc392 |
if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
|
|
Packit |
8dc392 |
else { x = xi; y = yi; z = zi; }
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
setOrder(p);
|
|
Packit |
8dc392 |
extract(M);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
setOrder(p);
|
|
Packit |
8dc392 |
extract(M);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline void Euler<T>::extract(const Quat<T> &q)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
extract(q.toMatrix33());
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
void Euler<T>::extract(const Matrix33<T> &M)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
angleOrder(i,j,k);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (_initialRepeated)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the first angle, x.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
x = Math<T>::atan2 (M[j][i], M[k][i]);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Remove the x rotation from M, so that the remaining
|
|
Packit |
8dc392 |
// rotation, N, is only around two axes, and gimbal lock
|
|
Packit |
8dc392 |
// cannot occur.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> r (0, 0, 0);
|
|
Packit |
8dc392 |
r[i] = (_parityEven? -x: x);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Matrix44<T> N;
|
|
Packit |
8dc392 |
N.rotate (r);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
|
|
Packit |
8dc392 |
M[1][0], M[1][1], M[1][2], 0,
|
|
Packit |
8dc392 |
M[2][0], M[2][1], M[2][2], 0,
|
|
Packit |
8dc392 |
0, 0, 0, 1);
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the other two angles, y and z, from N.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
|
|
Packit |
8dc392 |
y = Math<T>::atan2 (sy, N[i][i]);
|
|
Packit |
8dc392 |
z = Math<T>::atan2 (N[j][k], N[j][j]);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
else
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the first angle, x.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
x = Math<T>::atan2 (M[j][k], M[k][k]);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Remove the x rotation from M, so that the remaining
|
|
Packit |
8dc392 |
// rotation, N, is only around two axes, and gimbal lock
|
|
Packit |
8dc392 |
// cannot occur.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> r (0, 0, 0);
|
|
Packit |
8dc392 |
r[i] = (_parityEven? -x: x);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Matrix44<T> N;
|
|
Packit |
8dc392 |
N.rotate (r);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
|
|
Packit |
8dc392 |
M[1][0], M[1][1], M[1][2], 0,
|
|
Packit |
8dc392 |
M[2][0], M[2][1], M[2][2], 0,
|
|
Packit |
8dc392 |
0, 0, 0, 1);
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the other two angles, y and z, from N.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
|
|
Packit |
8dc392 |
y = Math<T>::atan2 (-N[i][k], cy);
|
|
Packit |
8dc392 |
z = Math<T>::atan2 (-N[j][i], N[j][j]);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (!_parityEven)
|
|
Packit |
8dc392 |
*this *= -1;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (!_frameStatic)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
T t = x;
|
|
Packit |
8dc392 |
x = z;
|
|
Packit |
8dc392 |
z = t;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
void Euler<T>::extract(const Matrix44<T> &M)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
angleOrder(i,j,k);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (_initialRepeated)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the first angle, x.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
x = Math<T>::atan2 (M[j][i], M[k][i]);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Remove the x rotation from M, so that the remaining
|
|
Packit |
8dc392 |
// rotation, N, is only around two axes, and gimbal lock
|
|
Packit |
8dc392 |
// cannot occur.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> r (0, 0, 0);
|
|
Packit |
8dc392 |
r[i] = (_parityEven? -x: x);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Matrix44<T> N;
|
|
Packit |
8dc392 |
N.rotate (r);
|
|
Packit |
8dc392 |
N = N * M;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the other two angles, y and z, from N.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
|
|
Packit |
8dc392 |
y = Math<T>::atan2 (sy, N[i][i]);
|
|
Packit |
8dc392 |
z = Math<T>::atan2 (N[j][k], N[j][j]);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
else
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the first angle, x.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
x = Math<T>::atan2 (M[j][k], M[k][k]);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Remove the x rotation from M, so that the remaining
|
|
Packit |
8dc392 |
// rotation, N, is only around two axes, and gimbal lock
|
|
Packit |
8dc392 |
// cannot occur.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> r (0, 0, 0);
|
|
Packit |
8dc392 |
r[i] = (_parityEven? -x: x);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Matrix44<T> N;
|
|
Packit |
8dc392 |
N.rotate (r);
|
|
Packit |
8dc392 |
N = N * M;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
// Extract the other two angles, y and z, from N.
|
|
Packit |
8dc392 |
//
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
|
|
Packit |
8dc392 |
y = Math<T>::atan2 (-N[i][k], cy);
|
|
Packit |
8dc392 |
z = Math<T>::atan2 (-N[j][i], N[j][j]);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (!_parityEven)
|
|
Packit |
8dc392 |
*this *= -1;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (!_frameStatic)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
T t = x;
|
|
Packit |
8dc392 |
x = z;
|
|
Packit |
8dc392 |
z = t;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
Matrix33<T> Euler<T>::toMatrix33() const
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
angleOrder(i,j,k);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> angles;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( _frameStatic ) angles = (*this);
|
|
Packit |
8dc392 |
else angles = Vec3<T>(z,y,x);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( !_parityEven ) angles *= -1.0;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T ci = Math<T>::cos(angles.x);
|
|
Packit |
8dc392 |
T cj = Math<T>::cos(angles.y);
|
|
Packit |
8dc392 |
T ch = Math<T>::cos(angles.z);
|
|
Packit |
8dc392 |
T si = Math<T>::sin(angles.x);
|
|
Packit |
8dc392 |
T sj = Math<T>::sin(angles.y);
|
|
Packit |
8dc392 |
T sh = Math<T>::sin(angles.z);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T cc = ci*ch;
|
|
Packit |
8dc392 |
T cs = ci*sh;
|
|
Packit |
8dc392 |
T sc = si*ch;
|
|
Packit |
8dc392 |
T ss = si*sh;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Matrix33<T> M;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( _initialRepeated )
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci;
|
|
Packit |
8dc392 |
M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
|
|
Packit |
8dc392 |
M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
else
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
|
|
Packit |
8dc392 |
M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
|
|
Packit |
8dc392 |
M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
return M;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
Matrix44<T> Euler<T>::toMatrix44() const
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
angleOrder(i,j,k);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> angles;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( _frameStatic ) angles = (*this);
|
|
Packit |
8dc392 |
else angles = Vec3<T>(z,y,x);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( !_parityEven ) angles *= -1.0;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T ci = Math<T>::cos(angles.x);
|
|
Packit |
8dc392 |
T cj = Math<T>::cos(angles.y);
|
|
Packit |
8dc392 |
T ch = Math<T>::cos(angles.z);
|
|
Packit |
8dc392 |
T si = Math<T>::sin(angles.x);
|
|
Packit |
8dc392 |
T sj = Math<T>::sin(angles.y);
|
|
Packit |
8dc392 |
T sh = Math<T>::sin(angles.z);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T cc = ci*ch;
|
|
Packit |
8dc392 |
T cs = ci*sh;
|
|
Packit |
8dc392 |
T sc = si*ch;
|
|
Packit |
8dc392 |
T ss = si*sh;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Matrix44<T> M;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( _initialRepeated )
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
M[i][i] = cj; M[j][i] = sj*si; M[k][i] = sj*ci;
|
|
Packit |
8dc392 |
M[i][j] = sj*sh; M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
|
|
Packit |
8dc392 |
M[i][k] = -sj*ch; M[j][k] = cj*sc+cs; M[k][k] = cj*cc-ss;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
else
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
|
|
Packit |
8dc392 |
M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
|
|
Packit |
8dc392 |
M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
return M;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
Quat<T> Euler<T>::toQuat() const
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
Vec3<T> angles;
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
angleOrder(i,j,k);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( _frameStatic ) angles = (*this);
|
|
Packit |
8dc392 |
else angles = Vec3<T>(z,y,x);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( !_parityEven ) angles.y = -angles.y;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T ti = angles.x*0.5;
|
|
Packit |
8dc392 |
T tj = angles.y*0.5;
|
|
Packit |
8dc392 |
T th = angles.z*0.5;
|
|
Packit |
8dc392 |
T ci = Math<T>::cos(ti);
|
|
Packit |
8dc392 |
T cj = Math<T>::cos(tj);
|
|
Packit |
8dc392 |
T ch = Math<T>::cos(th);
|
|
Packit |
8dc392 |
T si = Math<T>::sin(ti);
|
|
Packit |
8dc392 |
T sj = Math<T>::sin(tj);
|
|
Packit |
8dc392 |
T sh = Math<T>::sin(th);
|
|
Packit |
8dc392 |
T cc = ci*ch;
|
|
Packit |
8dc392 |
T cs = ci*sh;
|
|
Packit |
8dc392 |
T sc = si*ch;
|
|
Packit |
8dc392 |
T ss = si*sh;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
T parity = _parityEven ? 1.0 : -1.0;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Quat<T> q;
|
|
Packit |
8dc392 |
Vec3<T> a;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( _initialRepeated )
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
a[i] = cj*(cs + sc);
|
|
Packit |
8dc392 |
a[j] = sj*(cc + ss) * parity,
|
|
Packit |
8dc392 |
a[k] = sj*(cs - sc);
|
|
Packit |
8dc392 |
q.r = cj*(cc - ss);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
else
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
a[i] = cj*sc - sj*cs,
|
|
Packit |
8dc392 |
a[j] = (cj*ss + sj*cc) * parity,
|
|
Packit |
8dc392 |
a[k] = cj*cs - sj*sc;
|
|
Packit |
8dc392 |
q.r = cj*cc + sj*ss;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
q.v = a;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
return q;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline bool
|
|
Packit |
8dc392 |
Euler<T>::legal(typename Euler<T>::Order order)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
return (order & ~Legal) ? false : true;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
typename Euler<T>::Order
|
|
Packit |
8dc392 |
Euler<T>::order() const
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (_parityEven) foo |= 0x0100;
|
|
Packit |
8dc392 |
if (_initialRepeated) foo |= 0x0010;
|
|
Packit |
8dc392 |
if (_frameStatic) foo++;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
return (Order)foo;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
inline void Euler<T>::setOrder(typename Euler<T>::Order p)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis
|
|
Packit |
8dc392 |
!(p & 0x1), // static?
|
|
Packit |
8dc392 |
!!(p & 0x100), // permutation even?
|
|
Packit |
8dc392 |
!!(p & 0x10)); // initial repeats?
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
void Euler<T>::set(typename Euler<T>::Axis axis,
|
|
Packit |
8dc392 |
bool relative,
|
|
Packit |
8dc392 |
bool parityEven,
|
|
Packit |
8dc392 |
bool firstRepeats)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
_initialAxis = axis;
|
|
Packit |
8dc392 |
_frameStatic = !relative;
|
|
Packit |
8dc392 |
_parityEven = parityEven;
|
|
Packit |
8dc392 |
_initialRepeated = firstRepeats;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
x = euler.x;
|
|
Packit |
8dc392 |
y = euler.y;
|
|
Packit |
8dc392 |
z = euler.z;
|
|
Packit |
8dc392 |
_initialAxis = euler._initialAxis;
|
|
Packit |
8dc392 |
_frameStatic = euler._frameStatic;
|
|
Packit |
8dc392 |
_parityEven = euler._parityEven;
|
|
Packit |
8dc392 |
_initialRepeated = euler._initialRepeated;
|
|
Packit |
8dc392 |
return *this;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
x = v.x;
|
|
Packit |
8dc392 |
y = v.y;
|
|
Packit |
8dc392 |
z = v.z;
|
|
Packit |
8dc392 |
return *this;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template<class T>
|
|
Packit |
8dc392 |
std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
char a[3] = { 'X', 'Y', 'Z' };
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
const char* r = euler.frameStatic() ? "" : "r";
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
euler.angleOrder(i,j,k);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if ( euler.initialRepeated() ) k = i;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
return o << "("
|
|
Packit |
8dc392 |
<< euler.x << " "
|
|
Packit |
8dc392 |
<< euler.y << " "
|
|
Packit |
8dc392 |
<< euler.z << " "
|
|
Packit |
8dc392 |
<< a[i] << a[j] << a[k] << r << ")";
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template <class T>
|
|
Packit |
8dc392 |
float
|
|
Packit |
8dc392 |
Euler<T>::angleMod (T angle)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
angle = fmod(T (angle), T (2 * M_PI));
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (angle < -M_PI) angle += 2 * M_PI;
|
|
Packit |
8dc392 |
if (angle > +M_PI) angle -= 2 * M_PI;
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
return angle;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template <class T>
|
|
Packit |
8dc392 |
void
|
|
Packit |
8dc392 |
Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
Vec3<T> d = xyzRot - targetXyzRot;
|
|
Packit |
8dc392 |
xyzRot[0] = targetXyzRot[0] + angleMod(d[0]);
|
|
Packit |
8dc392 |
xyzRot[1] = targetXyzRot[1] + angleMod(d[1]);
|
|
Packit |
8dc392 |
xyzRot[2] = targetXyzRot[2] + angleMod(d[2]);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template <class T>
|
|
Packit |
8dc392 |
void
|
|
Packit |
8dc392 |
Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
|
|
Packit |
8dc392 |
Order order)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
int i,j,k;
|
|
Packit |
8dc392 |
Euler<T> e (0,0,0, order);
|
|
Packit |
8dc392 |
e.angleOrder(i,j,k);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
simpleXYZRotation(xyzRot, targetXyzRot);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> otherXyzRot;
|
|
Packit |
8dc392 |
otherXyzRot[i] = M_PI+xyzRot[i];
|
|
Packit |
8dc392 |
otherXyzRot[j] = M_PI-xyzRot[j];
|
|
Packit |
8dc392 |
otherXyzRot[k] = M_PI+xyzRot[k];
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
simpleXYZRotation(otherXyzRot, targetXyzRot);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
Vec3<T> d = xyzRot - targetXyzRot;
|
|
Packit |
8dc392 |
Vec3<T> od = otherXyzRot - targetXyzRot;
|
|
Packit |
8dc392 |
T dMag = d.dot(d);
|
|
Packit |
8dc392 |
T odMag = od.dot(od);
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
if (odMag < dMag)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
xyzRot = otherXyzRot;
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
template <class T>
|
|
Packit |
8dc392 |
void
|
|
Packit |
8dc392 |
Euler<T>::makeNear (const Euler<T> &target)
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
Vec3<T> xyzRot = toXYZVector();
|
|
Packit |
8dc392 |
Vec3<T> targetXyz;
|
|
Packit |
8dc392 |
if (order() != target.order())
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
Euler<T> targetSameOrder = Euler<T>(target, order());
|
|
Packit |
8dc392 |
targetXyz = targetSameOrder.toXYZVector();
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
else
|
|
Packit |
8dc392 |
{
|
|
Packit |
8dc392 |
targetXyz = target.toXYZVector();
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
nearestRotation(xyzRot, targetXyz, order());
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
setXYZVector(xyzRot);
|
|
Packit |
8dc392 |
}
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
|
|
Packit |
8dc392 |
#pragma warning(default:4244)
|
|
Packit |
8dc392 |
#endif
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
|
|
Packit |
8dc392 |
#endif // INCLUDED_IMATHEULER_H
|