///////////////////////////////////////////////////////////////////////////////
//
// ## ######
// ###### ### Based on:
// ## ###############
// ########## # # # Shark 3D Engine (www.shark3d.com)
// ########
// ######### # # # Copyright (c) 1996-2001 Spinor GmbH.
// ## ##########
// ##
//
///////////////////////////////////////////////////////////////////////////////
//
// G A M E.O.N.E - LOW LEVEL LIB V1.0
// Copyright (C) 2001 LEVEL ONE ENTERTAINMENT,
// Licensed under the terms of LGPL.
//:---------------------------------------------------------------------------
//:Description
//
// LOW LEVEL 3D VEC3 UTIL HEADERFILE
//
//:---------------------------------------------------------------------------
#ifndef _LL3D_VEC3_H
#define _LL3D_VEC3_H
#include "ll3d_math.h"
namespace ll3d {
template<typename T> struct CVec3;
template<typename T> struct CMat3x3;
template<typename T> struct CQuat;
typedef CVec3<float> CVec3f;
typedef const CVec3f CVec3f_c;
typedef CVec3f &CVec3f_r;
typedef const CVec3f &CVec3f_cr;
typedef CVec3<double> CVec3d;
typedef const CVec3d CVec3d_c;
typedef CVec3d &CVec3d_r;
typedef const CVec3d &CVec3d_cr;
typedef CMat3x3<float> CMat3x3f;
typedef const CMat3x3f CMat3x3f_c;
typedef CMat3x3f &CMat3x3f_r;
typedef const CMat3x3f &CMat3x3f_cr;
typedef CMat3x3<double> CMat3x3d;
typedef const CMat3x3d CMat3x3d_c;
typedef CMat3x3d &CMat3x3d_r;
typedef const CMat3x3d &CMat3x3d_cr;
typedef CQuat<float> CQuatf;
typedef const CQuatf CQuatf_c;
typedef CQuatf &CQuatf_r;
typedef const CQuatf &CQuatf_cr;
typedef CQuat<double> CQuatd;
typedef const CQuatd CQuatd_c;
typedef CQuatd &CQuatd_r;
typedef const CQuatd &CQuatd_cr;
/*
* 3D Vector
*/
template<typename T>
struct CVec3
{
T m_x, m_y, m_z;
CVec3<T>(); // Default constructor. Note that the components are not initialized.
CVec3<T>(T s); // Set each component to the same value.
CVec3<T>(T x, T y, T z); // Constructor
CVec3<T>(const CVec3<T> &v); // Copy constructor
CVec3<T> &operator=(const CVec3<T> &v); // Assignment operator.
CVec3<T> operator+() const;
CVec3<T> operator-() const; // Negation operator.
CVec3<T> &operator+=(const CVec3<T> &v);
CVec3<T> &operator-=(const CVec3<T> &v);
CVec3<T> &operator*=(T s);
CVec3<T> &operator/=(T s);
T GetSqLen() const; // Square of the vector.
T GetLen() const; // Square of the vector.
CVec3<T> GetNormalized() const; // Normalized to unit length.
CVec3<T> GetNormalizedTo(T Len) const; // Normalized to length
bool IsNull() const; // Are all components null?
bool IsFinite() const; // Are all components finit values?
// Linear interpolation between two vectors.
// Returns (*this) + (v - (*this)) * Frac.
CVec3<T> InterpLinear(const CVec3<T> &v, T Frac) const;
// Project v onto Dir. It is
// v.GetProjAlong() = Dir * (Vec.GetSqLen() / (Vec * Dir)).
// It is except for rounding errors
// v = v.GetProjOrtho() + v.GetProjAlong().
CVec3<T> GetProjAlong(const CVec3<T> &Dir) const;
// Project v onto the plane orthogonal to Dir.
// It is v.GetProjOrtho() = v - v.GetProjAlong().
// It is except for rounding errors
// v = v.GetProjOrtho() + v.GetProjAlong().
CVec3<T> GetProjOrtho(const CVec3<T> &Dir) const;
// Get constraint vector of v onto Dir. It is
// v.GetConstrAlong(Dir) = Dir * (v.GetSqLen() / (Vec * Dir)).
CVec3<T> GetConstrAlong(const CVec3<T> &Dir) const;
// Get constraint vector v onto the plane
// orthogonal to Dir.
CVec3<T> GetConstrOrtho(const CVec3<T> &Dir) const;
// Vector divided by its squared length.
// v.GetReziproc() = v / v.GetSqLen().
CVec3<T> GetReziproc() const;
CVec3<T> GetAbsComp() const; // Return for each component the absolute value.
CVec3<T> GetSignComp() const; // Return the sign of each component.
T GetMinComp() const; // Return the smallest component.
T GetMaxComp() const; // Return the largest component.
T GetSumAbsComp() const; // Return the sum of the absolute value of all components.
T GetMaxAbsComp() const; // Return the maximum of the absolute value of all components.
CVec3<T> &SetCompToMin(const CVec3<T> &v); // Set to the minimum component wise.
CVec3<T> &SetCompToMax(const CVec3<T> &v); // Set to the maximum component wise.
CVec3<T> CompMin(const CVec3<T> &v) const;
CVec3<T> CompMax(const CVec3<T> &v) const;
CVec3<T> CompMin(const CVec3<T> &v2, const CVec3<T> &v3) const;
CVec3<T> CompMax(const CVec3<T> &v2, const CVec3<T> &v3) const;
CVec3<T> operator+(const CVec3<T> &v) const;
CVec3<T> operator-(const CVec3<T> &v) const;
CVec3<T> operator*(T s) const; // Scalar multiplication.
CVec3<T> operator/(T s) const; // Scalar division.
T operator*(const CVec3<T> &v) const; // Scalar product.
CVec3<T> operator^(const CVec3<T> &v) const; // Cross product of two vectors.
CVec3<T> operator*(const CMat3x3<T> &m) const;
bool operator==(const CVec3<T> &v) const; // Equality.
bool operator!=(const CVec3<T> &v) const; // Inequality.
int CompareComp(const CVec3<T> &v) const; // Defines an order on vectors.
T Area(const CVec3<T> &v) const ;// Area spanned by v1 AND v2.
// Volume spanned by v1, v2 and v3.
// Guaranteed to be null if two vectors are equal.
T Volume(const CVec3<T> &v2, const CVec3<T> &v3) const;
CVec3<T> CompProd(const CVec3<T> &v) const; // Component wise product.
CVec3<T> CompQuot(const CVec3<T> &v) const; // Component wise division.
CVec3<T> CompReziproc() const; // Reziproc of each component.
bool IsCosAngleAbove(const CVec3<T> &v, T c) const; // Returns true if cos(angle_between(v1, v2)) > c
bool IsEachCompNe(const CVec3<T> &v) const;
bool IsEachCompLt(const CVec3<T> &v) const;
bool IsEachCompLe(const CVec3<T> &v) const;
bool IsEachCompGt(const CVec3<T> &v) const;
bool IsEachCompGe(const CVec3<T> &v) const;
bool IsAnyCompEq(const CVec3<T> &v) const;
bool IsAnyCompLt(const CVec3<T> &v) const;
bool IsAnyCompLe(const CVec3<T> &v) const;
bool IsAnyCompGt(const CVec3<T> &v) const;
bool IsAnyCompGe(const CVec3<T> &v) const;
// Scalar multiplication with t, cross product from left with x,y,z.
// Return [v * q.t, v ^ q.v].
// Warning! This operator does not rotate and scale the vector
// by the scaled rotation representated by the quaternion.
// To perform this, you must use the matrix
// returned by CQuat::GetMat.
CVec3<T> operator*(const CQuat<T> &q) const;
};
/*
* 3x3 Matrix
*/
template<typename T>
struct CMat3x3
{
T m_xx, m_xy, m_xz,
m_yx, m_yy, m_yz,
m_zx, m_zy, m_zz;
CMat3x3<T>(); // Default constructor. Note that the components are not initialized.
// Initializating with multiple of unity matrix.
// Also allows to use 0 as null matrix and 1 as unity matrix.
CMat3x3<T>(T s);
// Constructor.
CMat3x3<T>(
T xx, T xy, T xz,
T yx, T yy, T yz,
T zx, T zy, T zz);
CMat3x3<T>(const CMat3x3<T> &m); // Copy constructor.
CMat3x3<T> &operator=(const CMat3x3<T> &m); // Assignment operator.
CMat3x3<T> operator+() const;
CMat3x3<T> operator-() const; // Negation operator.
CMat3x3<T> &operator+=(const CMat3x3<T> &m);
CMat3x3<T> &operator-=(const CMat3x3<T> &m);
CMat3x3<T> &operator*=(const CMat3x3<T> &m);
CMat3x3<T> &operator*=(T s);
CMat3x3<T> &operator/=(T s);
CMat3x3<T> GetTransposed() const; // Returns the transposed matrix.
// Return the transposed inverse matrix
// times the determinant of the matrix.
CMat3x3<T> GetSubDet() const;
CMat3x3<T> GetInv() const; // Return the inverse matrix.
T GetSqLen() const; // Returns the sum of the square if all components.
T GetLen() const; // Returns the root of the sum of the square of all components.
// Returns the determinant.
// Guaranteed to be null if two rows or colums are equal.
T GetDet() const;
T GetTrace() const; // Returns the sum of the diagonal elements.
// Returns the dual vector of the matrix.
// The dual vector is the contraction of the levicita tensor
// with the matrix interpretated as tensor of second rank.
// v_a = e_abc M^bc.
CVec3<T> GetDual() const;
bool IsNull() const; // Are all components null?
bool IsFinite() const; // Are all components finite?
CVec3<T> GetRowX() const; // Return the vector build of m_xx, m_xy, m_xz.
CVec3<T> GetRowY() const; // Return the vector build of m_yx, m_yy, m_yz.
CVec3<T> GetRowZ() const; // Return the vector build of m_zx, m_zy, m_zz.
CVec3<T> GetColX() const; // Return the vector build of m_xx, m_yx, m_zx.
CVec3<T> GetColY() const; // Return the vector build of m_xy, m_yy, m_zy.
CVec3<T> GetColZ() const; // Return the vector build of m_xz, m_yz, m_zz.
void SetRowX(const CVec3<T> &v);
void SetRowY(const CVec3<T> &v);
void SetRowZ(const CVec3<T> &v);
void SetColX(const CVec3<T> &v);
void SetColY(const CVec3<T> &v);
void SetColZ(const CVec3<T> &v);
CMat3x3<T> GetAbsComp() const;// Builds the absolute value of each component.
T GetSumAbsComp() const;// Returns the sum of the absolute values of each component.
T GetMaxAbsComp() const;// Returns the maximal absolute value of all components.
CVec3<T> GetSumAbsRows() const;// Returns per the sum of the absolute values of the components of each row.
CVec3<T> GetSumAbsCols() const;// Returns per the sum of the absolute values of the components of each column.
CVec3<T> GetMaxAbsRows() const;// Returns per the maximum of the absolute values of the components of each row.
CVec3<T> GetMaxAbsCols() const;// Returns per the maximum of the absolute values of the components of each column.
CVec3<T> GetLenRows() const;// Returns the length of the row vectors.
CVec3<T> GetLenCols() const;// Returns the length of the column vectors.
CMat3x3<T> GetScaledRows(const CVec3<T> &v) const;// Returns the length of the column vectors.
CMat3x3<T> GetScaledCols(const CVec3<T> &v) const;// Multiplies each column the the corresponding component of v.
// Tensor product of two vectors.
// This is no global function to explicitely exclude type conversions
// for the first argument.
static CMat3x3<T> ByTensorProd(const CVec3<T> &v1, const CVec3<T> &v2);
// The dual matrix of a vector is the contraction of the levicita tensor
// with the vector.
// M_ab = e_abc v^c.
static CMat3x3<T> ByDual(const CVec3<T> &v);
static CMat3x3<T> ByDiag(const CVec3<T> &v);// Create a diagonal matrix.
static CMat3x3<T> ByDiag(T x, T y, T z);// Create a diagonal matrix.
static CMat3x3<T> ByRows(
const CVec3<T> &vx,
const CVec3<T> &vy,
const CVec3<T> &vz); // Create a matrix by its row vectors.
static CMat3x3<T> ByCols(
const CVec3<T> &vx,
const CVec3<T> &vy,
const CVec3<T> &vz);// Create a matrix by its column vectors.
CMat3x3<T> operator+(const CMat3x3<T> &m) const;
CMat3x3<T> operator-(const CMat3x3<T> &m) const;
CMat3x3<T> operator*(T s) const;// Scalar multiplication.
CMat3x3<T> operator/(T s) const;
CMat3x3<T> operator*(const CMat3x3<T> &m) const;// Matrix multiplication.
CVec3<T> operator*(const CVec3<T> &v) const;
// Multiply the vector x,y,z with the matrix from left.
// Return [q.t, m * q.v]
CQuat<T> operator*(const CQuat<T> &q);
bool operator==(const CMat3x3<T> &m) const;
bool operator!=(const CMat3x3<T> &m) const;
// Defines an order on vectors.
int CompareComp(const CMat3x3<T> &m) const;
};
/*
* Quaternion
*
* A quaternion has four components (t, x, y, z).
* It represents a concatication of a rotation and a scaling
* in the following way.
* The direction of the vector (x, y, z) is the rotation axis.
* The length of the vector (x, y, z) is sqrt(scale) * sin(angle/2)
* and t is sqrt(scale) * cos(angle/2),
* where angle is the rotation angle, and scale is the scaling factor.
*
*/
template<typename T>
struct CQuat
{
T m_t, m_x, m_y, m_z;
// Default constructor. Note that the components are not initialized.
CQuat<T>();
// Initialize with a multiple of the unity quaternion.
// Also allows to use 0 as null quaternion.
CQuat<T>(T s);
CQuat<T>(T t, T x, T y, T z);
CQuat<T>(T t, const CVec3<T> &v);
CQuat<T>(const CQuat<T> &q);// Copy constructor.
CQuat<T> &operator=(const CQuat<T> &q);// Assign operator.
CVec3<T> GetVec() const;// Return the components x,y,z as vector.
void SetVec(const CVec3<T> &v);// Set the x,y,z components and let t unchanged.
CQuat<T> operator+() const;
CQuat<T> operator-() const; // Negation.
CQuat<T> &operator+=(const CQuat<T> &q);
CQuat<T> &operator-=(const CQuat<T> &q);
CQuat<T> &operator*=(const CQuat<T> &q); // Quaternion multiplication.
CQuat<T> &operator*=(T s);
CQuat<T> &operator/=(T s);
CQuat<T> GetAbs() const;// If t < 0, negate.
// Negate x,y,z.
// Geometrically this means to reverse the rotation
// but keep the scaling.
// Mathematically, this is the star operator.
// See also @ident{CQuat::GetRevMat}.
CQuat<T> GetRev() const;
// Get the inverse quaternion.
CQuat<T> GetInv() const;
// The sum of the square of each component.
// This is the scaling factor.
T GetSqLen() const;
// The root of the sum of the square of each component.
// This is the square root of the scaling factor.
T GetLen() const;
// The half angle of the rotation.
// See also @ident{CQuat::GetRotAngle}.
T GetQuatAngle() const;
T GetRotAngle() const;// The angle of the rotation.
CQuat<T> GetNormalized() const;// Normalize to length one.
CQuat<T> GetNormalizedTo(T Len) const;// Normalize to length
// Square root of the quaternion.
// Has same axis, half angle, and root of length.
CQuat<T> GetSqRoot() const;
// Power of a quaternion, where the result is normalized to one.
// Has same axis, the angle multiplied by the exponent,
// and the length always one.
// If *this is zero, the result is 1.
// This function is faster than the full CQuat::PowerTo
CQuat<T> NormalizedPowerTo(T Exponent) const;
// Power of a quaternion.
// Has same axis, the angle multiplied by the exponent,
// and the length raised to the power of the exponent.
// This function is slower than CQuat::NormalizedPowerTo.
CQuat<T> PowerTo(T Exponent) const;
bool IsNull() const;// Are all components null?
bool IsFinite() const;// Are all components finite?
CMat3x3<T> GetMat() const;// Builds the corresponding scaled rotation matrix.
// Builds the reverse of the corresponding scaled rotation matrix.
// See also CQuat::GetRev.
CMat3x3<T> GetRevMat() const;
// Builds the inverse of the corresponding scaled rotation matrix.
// See also CQuat::GetInv.
CMat3x3<T> GetInvMat() const;
// Return rotation around Axis by angle Angle radians.
static CQuat<T> ByRot(
const CVec3<T> &Axis, T Angle);
// Return scaled rotation around Axis
// by angle Angle radians and scaled by SqLen.
static CQuat<T> ByScaledRot(
const CVec3<T> &Axis, T Angle, T Scale);
// Returns the shortest scaled rotation mapping
// From into To.
// Also see CQuat::ByPathAxis
// and CQuat::ByPathAlso.
static CQuat<T> ByPathShortest(
const CVec3<T> &From,
const CVec3<T> &To);
// Returns the rotation around Axis
// mapping the plane spanned by Axis and From
// into the plane spanned by Axis and To.
// The resulting quaternion has no scale, so it has always length one.
static CQuat<T> ByPathAxis(
const CVec3<T> &Axis,
const CVec3<T> &AlsoFrom,
const CVec3<T> &AlsoTo);
// Returns the scaled rotation mapping From into To
// but also mapping the plane spanned by From and AlsoFrom
// into the plane spanned by To and AlsoTo.
// Roughly speaking, it mappes From into To,
// but also tries to map AlsoFrom into AlsoTo
// as far as possible.
static CQuat<T> ByPathAlso(const CVec3<T> &From,
const CVec3<T> &To,
const CVec3<T> &AlsoFrom,
const CVec3<T> &AlsoTo);
// Some kind of inverse of CQuat::GetMat.
// For a quaternion q with must have t not near 0
// is CQuat<T>::ByBestOfMat(q.GetMat()) = q.GetAbs().
// Returns a rotation-invariant result for matrices
// which are no scaled rotation.
// It uses the formulars
// SqRoot(0.5 * (m.GetTrace() - s), v = - 0.5 * m.GetDual())
// Warning! Degenerates for 180 degree rotations (hence t -> 0).
static CQuat<T> ByBestOfMat(const CMat3x3<T> &m);
// Some kind of inverse of @ident{CQuat::GetMat}.
// For a quaternion q
// is CQuat<T>::ByRotMat(q.GetMat()) = q.GetAbs().
// This function has no degeneration point for scaledrotation matrices.
// Warning! Generates crazy, non-rotation-invariant results
// for non-scaledrotation matrices.
static CQuat<T> ByRotMat(const CMat3x3<T> &m);
// Returns the total quaternion when concatenating
// the following rotations:
//
// Pan radians around the x-axis.
// Tilt radians around the y-axis.
// Roll radians around the z-axis.
// This is equivalent to
//
// {
// CQuat::ByRotXYZ(Pan, Tilt, Roll)
// = CQuat::ByRot(CVec3(1, 0, 0), Pan)
// * CQuat::ByRot(CVec3(0, 1, 0), Tilt)
// * CQuat::ByRot(CVec3(0, 0, 1), Roll)
// }
// Assuming that a vector is rotated by multiplying the rotation matrix
// from left as in
// {
// v -> CQuat::ByRotXYZ(Pan, Tilt, Roll) * v;
// }
// pan, tilt and roll
// have the usual geometrical interpretation.
// The inverse operation is CQuat::GetRotAngleXYZ.
// If the rotations around the axis should be performed
// around the axes in different order, you can still use this function,
// but have to permutate the x,y and z component
// of the resulting quaternion.
// For example, to rotate
// { By Pan around y-axis (instead of x-axis). }
// { By Tilt around the x-axis (instead of y-axis). }
// { By Roll around the z-axis. }
// calculate CQuat::ByRotXYZ(Pan, Tilt, Roll),
// and swap the x and y component of the resulting quaternion.
static CQuat<T> ByRotXYZ(T Pan, T Tilt, T Roll);
// Calculates three rotation angles pan, tilt roll.
// This is the inverse to CQuat::ByRotXYZ.
void GetRotAngleXYZ(T &Pan, T &Tilt, T &Roll);
CQuat<T> operator+(const CQuat<T> &q) const;
CQuat<T> operator-(const CQuat<T> &q) const;
CQuat<T> operator*(T s) const; // Scalar multiplication.
CQuat<T> operator/(T s) const; // Scalar division.
CQuat<T> operator*(const CQuat<T> &q) const; // Quaternion multiplication.
// Multiply the vector x,y,z with the matrix from right.
// Return [q.t, q.v * m]
CQuat<T> operator*(const CMat3x3<T> &m) const;
// Scalar multiplication with t, cross product from right with x,y,z.
// Return [v * q.t, v ^ q.v].
// Warning! This operator does not rotate and scale the vector
// by the scaled rotation representated by the quaternion.
// To perform this, you must use the matrix
// returned by CQuat::GetMat.
CVec3<T> operator*(const CVec3<T> &v) const;
bool operator==(const CQuat<T> &q) const;
bool operator!=(const CQuat<T> &q) const;
// Defines an order on vectors.
int CompareComp(const CQuat<T> &q) const;
// Scalar product:
// Sum of the product of each corresponding component.
// This is a rotation invariant quantity.
// The scalar product is negative if the quotient of the quaternions
// is negative.
T ScalarProd(const CQuat<T> &q) const;
T GetQuatAngleBetween(const CQuat<T> &q) const;// Rotation angle between two quaternions:
T GetRotAngleBetween(const CQuat<T> &q) const;// Rotation angle between two quaternions:
// interpolation
// Interpolates linear between two quaternions.
// Returns (*this) + (q - (*this)) * Frac.
// Linear interpolation means linear in the quaternion
// considered as vector.
// Frac == 0 returns *this,
// Frac == 1 returns q.
// Note that the corresponding matrix CQuat::GetMat
// is interpolated quadratically.
// If *this is near to q relative to their length,
// linear interpolation is aproximately equivalent
// to the exponential and geometric interpolations
// CQuat::InterpStrictExp,
// CQuat::InterpShortestExp,
// to CQuat::InterpStrictGeo, and
// CQuat::InterpShortestGeo
CQuat<T> InterpLinear(const CQuat<T> &q, T Frac) const;
// interpolation
// Interpolate exponential between two quaternions.
// Exponential interpolation means angle-linear interpolation
// of the rotation, and exponential interpolation of the scale,
// analogous to exponential interpolation of complex numbers.
// Frac == 0 returns q1.
// Frac == 1 returns q2.
// In contrast to CQuat::InterpShortestExp,
// interpolation pathes can be rotations up to 360 degrees
// due to the two-way representation of the same rotation
// by two quaternions.
// In contrast to CQuat::InterpStrictGeo,
// the scale is interpolated exponential.
// Both *this and q must not be zero or near zero,
// since the function degenerates at these points.
// If *this is near to q relative to their length,
// the interpolation is aproximately equivalent
// to linear interpolation CQuat::InterpLinear.
CQuat<T> InterpStrictExp(const CQuat<T> &q, T Frac) const;
// interpolation
// Interpolate exponential between two scaled rotations.
// Exponential interpolation means angle-linear interpolation
// of the rotation, and exponential interpolation of the scale,
// analogous to exponential interpolation of complex numbers.
// Frac == 0 returns +/- q1.
// Frac == 1 returns +/- q2.
// In contrast to CQuat::InterpStrictExp,
// this interpolation identifies the two quaternions representing
// the same scaled rotation, so that the interpolation path
// always is not more than 180 degree.
// In contrast to CQuat::InterpShortestExp.
// the scale is interpolated exponential.
// Both *this and q must not be zero or near zero,
// since the function degenerates at these points.
// If *this is near to q relative to their length,
// the interpolation is aproximately equivalent
// to linear interpolation CQuat::InterpLinear.
CQuat<T> InterpShortestExp(const CQuat<T> &q, T Frac) const;
// interpolation
// Interpolate geometrical between two quaternions.
// Geometrical interpolation means angle-linear interpolation
// of the rotation, and linear interpolation of the scale.
// Frac == 0} returns q1.
// Frac == 1} returns q2.
// In contrast to CQuat::InterpShortestGeo,
// interpolation pathes can be rotations up to 360 degrees
// due to the two-way representation of the same rotation
// by two quaternions.
// In contrast to CQuat::InterpStrictExp,
// the scale is interpolated linear.
// If *this or q is zero, the angle is kept constant,
// and only the scale is interpolated.
// If *this or q are not zero, they should be not near zero
// to avoid degeneration artifacts.
// If *this is near to q relative to their length,
// the interpolation is aproximately equivalent
// to linear interpolation CQuat::InterpLinear.
CQuat<T> InterpStrictGeo(const CQuat<T> &q, T Frac) const;
// interpolation
// Interpolate geometrical between two scaled rotations.
// Geometrical interpolation means angle-linear interpolation
// of the rotation, and linear interpolation of the scale.
// Frac == 0 returns +/- q1.
// Frac == 1 returns +/- q2.
// In contrast to CQuat::InterpStrictGeo,
// this interpolation identifies the two quaternions representing
// the same scaled rotation, so that the interpolation path
// always is not more than 180 degree.
// In contrast to CQuat::InterpShortestExp.
// the scale is interpolated linear.
// If *this or q is zero, the angle is kept constant,
// and only the scale is interpolated.
// If *this or q are not zero, they should be not near zero
// to avoid degeneration artifacts.
// If *this is near to q relative to their length,
// the interpolation is aproximately equivalent
// to linear interpolation CQuat::InterpLinear.
CQuat<T> InterpShortestGeo(const CQuat<T> &q, T Frac) const;
// Extract the rotation around the axis Dir.
// The rotation axis CQuat::GetVec is projected
// onto Dir. It is v.GetProjAlong(Dir)
// = CQuat(v.t, v.GetVec().GetProjAlong(Dir)).
// It is except for rounding errors
// v.GetProjAlong().GetVec() parallel to Dir.
// The function does not conserve the norm of the quaternion.
// See also CQuat::GetProjOrthoLeft
// and CQuat::GetProjOrthoRight.
CQuat<T> GetProjAlong(const CVec3<T> &Dir) const;
// Eliminate rotation around the axis Dir.
// It is except for rounding errors
// v = v.GetProjOrthoLeft() * v.GetProjAlong()
// and v.GetProjOrthoLeft().GetVec() * Ortho = 0.
// The function does not conserve the norm of the quaternion.
// See also CQuat::GetProjAlong
// and CQuat::GetProjOrthoRight.
CQuat<T> GetProjOrthoLeft(const CVec3<T> &Dir) const;
// Eliminate rotation around the axis Dir.
// It is except for rounding errors
// v = v.GetProjAlong() * v.GetProjOrthoRight()
// and v.GetProjOrthoRight().GetVec() * Ortho = 0.
// The function does not conserve the norm of the quaternion.
// See also CQuat::GetProjAlong
// and CQuat::GetProjOrthoLeft.
CQuat<T> GetProjOrthoRight(const CVec3<T> &Dir) const;
};
/*
* CVector Implementation
*/
template<typename T>
inline CVec3<T>::CVec3()
{
}
template<typename T>
inline CVec3<T>::CVec3(T s)
{
m_x = m_y = m_z = s;
}
template<typename T>
inline CVec3<T>::CVec3(T x, T y, T z)
{
m_x = x;
m_y = y;
m_z = z;
}
template<typename T>
inline CVec3<T>::CVec3(const CVec3<T> &v)
{
m_x = v.m_x;
m_y = v.m_y;
m_z = v.m_z;
}
template<typename T>
inline CVec3<T> &CVec3<T>::operator=(const CVec3<T> &v)
{
m_x = v.m_x;
m_y = v.m_y;
m_z = v.m_z;
return (*this);
}
template<typename T>
inline CVec3<T> CVec3<T>::operator+() const
{
return (*this);
}
template<typename T>
inline CVec3<T> CVec3<T>::operator-() const
{
return CVec3<T>(- m_x, - m_y, - m_z);
}
template<typename T>
inline CVec3<T> &CVec3<T>::operator+=(const CVec3<T> &v)
{
return (*this) = (*this) + v;
}
template<typename T>
inline CVec3<T> &CVec3<T>::operator-=(const CVec3<T> &v)
{
return (*this) = (*this) - v;
}
template<typename T>
inline CVec3<T> &CVec3<T>::operator*=(T s)
{
return (*this) = (*this) * s;
}
template<typename T>
inline CVec3<T> &CVec3<T>::operator/=(T s)
{
return (*this) = (*this) / s;
}
template<typename T>
inline T CVec3<T>::GetSqLen() const
{
return m_x * m_x + m_y * m_y + m_z * m_z;
}
template<typename T>
T CVec3<T>::GetLen() const
{
return Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
template<typename T>
CVec3<T> CVec3<T>::GetNormalized() const
{
T f = 1 / Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
return CVec3<T>(f * m_x, f * m_y, f * m_z);
}
template<typename T>
CVec3<T> CVec3<T>::GetNormalizedTo(T Len) const
{
T f = Len / Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
return CVec3<T>(f * m_x, f * m_y, f * m_z);
}
template<typename T>
inline bool CVec3<T>::IsNull() const
{
return m_x == 0 && m_y == 0 && m_z == 0;
}
template<typename T>
bool CVec3<T>::IsFinite() const
{
return IsFinite(m_x) && ll3d::IsFinite(m_y) && ll3d::IsFinite(m_z);
}
template<typename T>
CVec3<T> CVec3<T>::InterpLinear(const CVec3<T> &v, T Frac) const
{
return (*this) + (v - (*this)) * Frac;
}
template<typename T>
CVec3<T> CVec3<T>::GetProjAlong(const CVec3<T> &Dir) const
{
const CVec3<T> &Vec = (*this);
return Dir * ((Vec * Dir) / Dir.GetSqLen());
}
template<typename T>
CVec3<T> CVec3<T>::GetProjOrtho(const CVec3<T> &Dir) const
{
const CVec3<T> &Vec = (*this);
return Vec - Dir * ((Vec * Dir) / Dir.GetSqLen());
}
template<typename T>
CVec3<T> CVec3<T>::GetConstrAlong(const CVec3<T> &Dir) const
{
const CVec3<T> &Vec = (*this);
return Dir * (Vec.GetSqLen() / (Vec * Dir));
}
template<typename T>
CVec3<T> CVec3<T>::GetConstrOrtho(const CVec3<T> &Dir) const
{
const CVec3<T> &Vec = (*this);
T Frac = Vec.GetSqLen() / (Dir ^ Vec).GetSqLen();
return Vec * (Frac * Dir.GetSqLen()) - Dir * (Frac * (Dir * Vec));
}
template<typename T>
CVec3<T> CVec3<T>::GetReziproc() const
{
T f = 1 / (m_x * m_x + m_y * m_y + m_z * m_z);
return CVec3<T>(f * m_x, f * m_y, f * m_z);
}
template<typename T>
inline CVec3<T> CVec3<T>::GetAbsComp() const
{
return CVec3<T>(Abs(m_x), Abs(m_y), Abs(m_z));
}
template<typename T>
CVec3<T> CVec3<T>::GetSignComp() const
{
return CVec3<T>(Sign(m_x), Sign(m_y), Sign(m_z));
}
template<typename T>
T CVec3<T>::GetMinComp() const
{
return Min(m_x, m_y, m_z);
}
template<typename T>
T CVec3<T>::GetMaxComp() const
{
return Max(m_x, m_y, m_z);
}
template<typename T>
T CVec3<T>::GetSumAbsComp() const
{
return Abs(m_x) + Abs(m_y) + Abs(m_z);
}
template<typename T>
T CVec3<T>::GetMaxAbsComp() const
{
return Max(Abs(m_x), Abs(m_y), Abs(m_z));
}
template<typename T>
CVec3<T> CVec3<T>::CompMin(const CVec3<T> &v) const
{
return CVec3<T>(
Min(m_x, v.m_x),
Min(m_y, v.m_y),
Min(m_z, v.m_z));
}
template<typename T>
CVec3<T> CVec3<T>::CompMax(const CVec3<T> &v) const
{
return CVec3<T>(
Max(m_x, v.m_x),
Max(m_y, v.m_y),
Max(m_z, v.m_z));
}
template<typename T>
CVec3<T> CVec3<T>::CompMin(const CVec3<T> &v2,
const CVec3<T> &v3) const
{
return CVec3<T>(
Min(m_x, v2.m_x, v3.m_x),
Min(m_y, v2.m_y, v3.m_y),
Min(m_z, v2.m_z, v3.m_z));
}
template<typename T>
CVec3<T> CVec3<T>::CompMax(const CVec3<T> &v2,
const CVec3<T> &v3) const
{
return CVec3<T>(
Max(m_x, v2.m_x, v3.m_x),
Max(m_y, v2.m_y, v3.m_y),
Max(m_z, v2.m_z, v3.m_z));
}
template<typename T>
CVec3<T> &CVec3<T>::SetCompToMin(const CVec3<T> &v)
{
if(v.m_x < m_x) m_x = v.m_x;
if(v.m_y < m_y) m_y = v.m_y;
if(v.m_z < m_z) m_z = v.m_z;
return (*this);
}
template<typename T>
CVec3<T> &CVec3<T>::SetCompToMax(const CVec3<T> &v)
{
if(v.m_x > m_x) m_x = v.m_x;
if(v.m_y > m_y) m_y = v.m_y;
if(v.m_z > m_z) m_z = v.m_z;
return (*this);
}
///////////////////////////////////////////////////////////////////////////////
template<typename T>
inline CVec3<T> CVec3<T>::operator+(const CVec3<T> &v) const
{
return CVec3<T>(m_x + v.m_x, m_y + v.m_y, m_z + v.m_z);
}
template<typename T>
inline CVec3<T> CVec3<T>::operator-(const CVec3<T> &v) const
{
return CVec3<T>(m_x - v.m_x, m_y - v.m_y, m_z - v.m_z);
}
template<typename T>
inline CVec3<T> CVec3<T>::operator*(T s) const
{
return CVec3<T>(m_x * s, m_y * s, m_z * s);
}
template<typename T>
inline CVec3<T> CVec3<T>::operator/(T s) const
{
return CVec3<T>(m_x / s, m_y / s, m_z / s);
}
template<typename T>
inline T CVec3<T>::operator*(const CVec3<T> &v) const
{
return m_x * v.m_x + m_y * v.m_y + m_z * v.m_z;
}
template<typename T>
inline CVec3<T> CVec3<T>::operator^(const CVec3<T> &v) const
{
return CVec3<T>(
m_y * v.m_z - m_z * v.m_y,
m_z * v.m_x - m_x * v.m_z,
m_x * v.m_y - m_y * v.m_x);
}
template<typename T>
inline CVec3<T> CVec3<T>::operator*(const CMat3x3<T> &m) const
{
return CVec3<T>(
m_x * m.m_xx + m_y * m.m_yx + m_z * m.m_zx,
m_x * m.m_xy + m_y * m.m_yy + m_z * m.m_zy,
m_x * m.m_xz + m_y * m.m_yz + m_z * m.m_zz);
}
template<typename T>
inline bool CVec3<T>::operator==(const CVec3<T> &v) const
{
return m_x == v.m_x && m_y == v.m_y && m_z == v.m_z;
}
template<typename T>
inline bool CVec3<T>::operator!=(const CVec3<T> &v) const
{
return m_x != v.m_x || m_y != v.m_y || m_z != v.m_z;
}
template<typename T>
int CVec3<T>::CompareComp(const CVec3<T> &v) const
{
if(m_x < v.m_x)
return -1;
else if(m_x > v.m_x)
return 1;
else if(m_y < v.m_y)
return -1;
else if(m_y > v.m_y)
return 1;
else if(m_z < v.m_z)
return -1;
else if(m_z > v.m_z)
return 1;
else
return 0;
}
template<typename T>
T CVec3<T>::Area(const CVec3<T> &v) const
{
T x = m_y * v.m_z - m_z * v.m_y;
T y = m_z * v.m_x - m_x * v.m_z;
T z = m_x * v.m_y - m_y * v.m_x;
return Sqrt(x * x + y * y + z * z);
}
template<typename T>
T CVec3<T>::Volume(const CVec3<T> &v2,
const CVec3<T> &v3) const
{
// Not using subdeterminant expansion to guarantee
// that the result is exact null if two vectors are equal.
return m_x * v2.m_y * v3.m_z - m_x * v2.m_z * v3.m_y
+ m_y * v2.m_z * v3.m_x - m_y * v2.m_x * v3.m_z
+ m_z * v2.m_x * v3.m_y - m_z * v2.m_y * v3.m_x;
}
template<typename T>
inline CVec3<T> CVec3<T>::CompProd(const CVec3<T> &v) const
{
return CVec3<T>(m_x * v.m_x, m_y * v.m_y, m_z * v.m_z);
}
template<typename T>
inline CVec3<T> CVec3<T>::CompQuot(const CVec3<T> &v) const
{
return CVec3<T>(m_x / v.m_x, m_y / v.m_y, m_z / v.m_z);
}
template<typename T>
inline CVec3<T> CVec3<T>::CompReziproc() const
{
return CVec3<T>(1 / m_x, 1 / m_y, 1 / m_z);
}
template<typename T>
bool CVec3<T>::IsCosAngleAbove(const CVec3<T> &v, T c) const
{
const CVec3<T> &This = (*this);
T Scal = This * v;
T Sq1 = GetSqLen();
T Sq2 = v.GetSqLen();
return Scal * Abs(Scal) > c * Sq1 * Sq2;
}
template<typename T>
bool CVec3<T>::IsEachCompNe(const CVec3<T> &v) const
{
return m_x != v.m_x && m_y != v.m_y && m_z != v.m_z;
}
template<typename T>
bool CVec3<T>::IsEachCompLt(const CVec3<T> &v) const
{
return m_x < v.m_x && m_y < v.m_y && m_z < v.m_z;
}
template<typename T>
bool CVec3<T>::IsEachCompLe(const CVec3<T> &v) const
{
return m_x <= v.m_x && m_y <= v.m_y && m_z <= v.m_z;
}
template<typename T>
bool CVec3<T>::IsEachCompGt(const CVec3<T> &v) const
{
return m_x > v.m_x && m_y > v.m_y && m_z > v.m_z;
}
template<typename T>
bool CVec3<T>::IsEachCompGe(const CVec3<T> &v) const
{
return m_x >= v.m_x && m_y >= v.m_y && m_z >= v.m_z;
}
template<typename T>
bool CVec3<T>::IsAnyCompEq(const CVec3<T> &v) const
{
return m_x == v.m_x || m_y == v.m_y || m_z == v.m_z;
}
template<typename T>
bool CVec3<T>::IsAnyCompLt(const CVec3<T> &v) const
{
return m_x < v.m_x || m_y < v.m_y || m_z < v.m_z;
}
template<typename T>
bool CVec3<T>::IsAnyCompLe(const CVec3<T> &v) const
{
return m_x <= v.m_x || m_y <= v.m_y || m_z <= v.m_z;
}
template<typename T>
bool CVec3<T>::IsAnyCompGt(const CVec3<T> &v) const
{
return m_x > v.m_x || m_y > v.m_y || m_z > v.m_z;
}
template<typename T>
bool CVec3<T>::IsAnyCompGe(const CVec3<T> &v) const
{
return m_x >= v.m_x || m_y >= v.m_y || m_z >= v.m_z;
}
/*
* CMat3x3 Implementation
*/
template<typename T>
inline CMat3x3<T>::CMat3x3()
{
}
template<typename T>
inline CMat3x3<T>::CMat3x3(T s)
{
m_xx = s; m_xy = 0; m_xz = 0;
m_yx = 0; m_yy = s; m_yz = 0;
m_zx = 0, m_zy = 0; m_zz = s;
}
template<typename T>
inline CMat3x3<T>::CMat3x3(T xx, T xy, T xz,
T yx, T yy, T yz,
T zx, T zy, T zz)
{
m_xx = xx; m_xy = xy; m_xz = xz;
m_yx = yx; m_yy = yy; m_yz = yz;
m_zx = zx; m_zy = zy; m_zz = zz;
}
template<typename T>
inline CMat3x3<T>::CMat3x3(const CMat3x3<T> &m)
{
m_xx = m.m_xx; m_xy = m.m_xy; m_xz = m.m_xz;
m_yx = m.m_yx; m_yy = m.m_yy; m_yz = m.m_yz;
m_zx = m.m_zx; m_zy = m.m_zy; m_zz = m.m_zz;
}
template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator=(const CMat3x3<T> &m)
{
m_xx = m.m_xx; m_xy = m.m_xy; m_xz = m.m_xz;
m_yx = m.m_yx; m_yy = m.m_yy; m_yz = m.m_yz;
m_zx = m.m_zx; m_zy = m.m_zy; m_zz = m.m_zz;
return (*this);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator+() const
{
return (*this);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator-() const
{
return CMat3x3<T>(
- m_xx, - m_xy, - m_xz,
- m_yx, - m_yy, - m_yz,
- m_zx, - m_zy, - m_zz);
}
template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator+=(const CMat3x3<T> &m)
{
return (*this) = (*this) + m;
}
template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator-=(const CMat3x3<T> &m)
{
return (*this) = (*this) - m;
}
template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator*=(const CMat3x3<T> &m)
{
return (*this) = (*this) * m;
}
template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator*=(T s)
{
return (*this) = (*this) * s;
}
template<typename T>
inline CMat3x3<T> &CMat3x3<T>::operator/=(T s)
{
return (*this) = (*this) / s;
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::GetTransposed() const
{
return CMat3x3<T>(
m_xx, m_yx, m_zx,
m_xy, m_yy, m_zy,
m_xz, m_yz, m_zz);
}
template<typename T>
CMat3x3<T> CMat3x3<T>::GetSubDet() const
{
return CMat3x3<T>(
m_yy * m_zz - m_yz * m_zy,
m_yz * m_zx - m_yx * m_zz,
m_yx * m_zy - m_yy * m_zx,
m_zy * m_xz - m_zz * m_xy,
m_zz * m_xx - m_zx * m_xz,
m_zx * m_xy - m_zy * m_xx,
m_xy * m_yz - m_xz * m_yy,
m_xz * m_yx - m_xx * m_yz,
m_xx * m_yy - m_xy * m_yx);
}
template<typename T>
CMat3x3<T> CMat3x3<T>::GetInv() const
{
T sxx = m_yy * m_zz - m_yz * m_zy;
T sxy = m_yz * m_zx - m_yx * m_zz;
T sxz = m_yx * m_zy - m_yy * m_zx;
T syx = m_zy * m_xz - m_zz * m_xy;
T syy = m_zz * m_xx - m_zx * m_xz;
T syz = m_zx * m_xy - m_zy * m_xx;
T szx = m_xy * m_yz - m_xz * m_yy;
T szy = m_xz * m_yx - m_xx * m_yz;
T szz = m_xx * m_yy - m_xy * m_yx;
T d = m_xx * sxx + m_xy * sxy + m_xz * sxz;
T f = 1 / d;
return CMat3x3<T>(
f * sxx, f * syx, f * szx,
f * sxy, f * syy, f * szy,
f * sxz, f * syz, f * szz);
}
template<typename T>
T CMat3x3<T>::GetSqLen() const
{
return (m_xx * m_xx + m_xy * m_xy + m_xz * m_xz)
+ (m_yx * m_yx + m_yy * m_yy + m_yz * m_yz)
+ (m_zx * m_zx + m_zy * m_zy + m_zz * m_zz);
}
template<typename T>
T CMat3x3<T>::GetLen() const
{
return Sqrt(GetSqLen());
}
template<typename T>
T CMat3x3<T>::GetDet() const
{
// Not using subdeterminant expansion to guarantee
// that the result is exact null if two vectors are equal.
return m_xx * m_yy * m_zz - m_xx * m_yz * m_zy
+ m_xy * m_yz * m_zx - m_xy * m_yx * m_zz
+ m_xz * m_yx * m_zy - m_xz * m_yy * m_zx;
}
template<typename T>
inline T CMat3x3<T>::GetTrace() const
{
return m_xx + m_yy + m_zz;
}
template<typename T>
inline CVec3<T> CMat3x3<T>::GetDual() const
{
return CVec3<T>(m_yz - m_zy, m_zx - m_xz, m_xy - m_yx);
}
template<typename T>
bool CMat3x3<T>::IsNull() const
{
return m_xx == 0 && m_xy == 0 && m_xz == 0
&& m_yx == 0 && m_yy == 0 && m_yz == 0
&& m_zx == 0 && m_zy == 0 && m_zz == 0;
};
template<typename T>
bool CMat3x3<T>::IsFinite() const
{
return IsFinite(m_xx) && IsFinite(m_xy) && IsFinite(m_xz)
&& IsFinite(m_yx) && IsFinite(m_yy) && IsFinite(m_yz)
&& IsFinite(m_zx) && IsFinite(m_zy) && IsFinite(m_zz);
};
template<typename T>
CVec3<T> CMat3x3<T>::GetRowX() const
{
return CVec3<T>(m_xx, m_xy, m_xz);
}
template<typename T>
CVec3<T> CMat3x3<T>::GetRowY() const
{
return CVec3<T>(m_yx, m_yy, m_yz);
}
template<typename T>
CVec3<T> CMat3x3<T>::GetRowZ() const
{
return CVec3<T>(m_zx, m_zy, m_zz);
}
template<typename T>
CVec3<T> CMat3x3<T>::GetColX() const
{
return CVec3<T>(m_xx, m_yx, m_zx);
}
template<typename T>
CVec3<T> CMat3x3<T>::GetColY() const
{
return CVec3<T>(m_xy, m_yy, m_zy);
}
template<typename T>
CVec3<T> CMat3x3<T>::GetColZ() const
{
return CVec3<T>(m_xz, m_yz, m_zz);
}
template<typename T>
void CMat3x3<T>::SetRowX(const CVec3<T> &v)
{
m_xx = v.m_x;
m_xy = v.m_y;
m_xz = v.m_z;
}
template<typename T>
void CMat3x3<T>::SetRowY(const CVec3<T> &v)
{
m_yx = v.m_x;
m_yy = v.m_y;
m_yz = v.m_z;
}
template<typename T>
void CMat3x3<T>::SetRowZ(const CVec3<T> &v)
{
m_zx = v.m_x;
m_zy = v.m_y;
m_zz = v.m_z;
}
template<typename T>
void CMat3x3<T>::SetColX(const CVec3<T> &v)
{
m_xx = v.m_x;
m_yx = v.m_y;
m_zx = v.m_z;
}
template<typename T>
void CMat3x3<T>::SetColY(const CVec3<T> &v)
{
m_xy = v.m_x;
m_yy = v.m_y;
m_zy = v.m_z;
}
template<typename T>
void CMat3x3<T>::SetColZ(const CVec3<T> &v)
{
m_xz = v.m_x;
m_yz = v.m_y;
m_zz = v.m_z;
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::GetAbsComp() const
{
return CMat3x3<T>(
Abs(m_xx),
Abs(m_xy),
Abs(m_xz),
Abs(m_yx),
Abs(m_yy),
Abs(m_yz),
Abs(m_zx),
Abs(m_zy),
Abs(m_zz));
}
template<typename T>
inline T CMat3x3<T>::GetSumAbsComp() const
{
return Abs(m_xx) + Abs(m_xy) + Abs(m_xz)
+ Abs(m_yx) + Abs(m_yy) + Abs(m_yz)
+ Abs(m_zx) + Abs(m_zy) + Abs(m_zz);
}
template<typename T>
inline T CMat3x3<T>::GetMaxAbsComp() const
{
return Max(
Max(Abs(m_xx), Abs(m_xy), Abs(m_xz)),
Max(Abs(m_yx), Abs(m_yy), Abs(m_yz)),
Max(Abs(m_zx), Abs(m_zy), Abs(m_zz)));
}
template<typename T>
CVec3<T> CMat3x3<T>::GetSumAbsRows() const
{
return CVec3<T>(
Abs(m_xx) + Abs(m_xy) + Abs(m_xz),
Abs(m_yx) + Abs(m_yy) + Abs(m_yz),
Abs(m_zx) + Abs(m_zy) + Abs(m_zz));
}
template<typename T>
CVec3<T> CMat3x3<T>::GetSumAbsCols() const
{
return CVec3<T>(
Abs(m_xx) + Abs(m_yx) + Abs(m_zx),
Abs(m_xy) + Abs(m_yy) + Abs(m_zy),
Abs(m_xz) + Abs(m_yz) + Abs(m_zz));
}
template<typename T>
CVec3<T> CMat3x3<T>::GetMaxAbsRows() const
{
return CVec3<T>(
Max(Abs(m_xx), Abs(m_xy), Abs(m_xz)),
Max(Abs(m_yx), Abs(m_yy), Abs(m_yz)),
Max(Abs(m_zx), Abs(m_zy), Abs(m_zz)));
}
template<typename T>
CVec3<T> CMat3x3<T>::GetMaxAbsCols() const
{
return CVec3<T>(
Max(Abs(m_xx), Abs(m_yx), Abs(m_zx)),
Max(Abs(m_xy), Abs(m_yy), Abs(m_zy)),
Max(Abs(m_xz), Abs(m_yz), Abs(m_zz)));
}
template<typename T>
CVec3<T> CMat3x3<T>::GetLenRows() const
{
return CVec3<T>(
Sqrt(m_xx * m_xx + m_xy * m_xy + m_xz * m_xz),
Sqrt(m_yx * m_yx + m_yy * m_yy + m_yz * m_yz),
Sqrt(m_zx * m_zx + m_zy * m_zy + m_zz * m_zz));
}
template<typename T>
CVec3<T> CMat3x3<T>::GetLenCols() const
{
return CVec3<T>(
Sqrt(m_xx * m_xx + m_yx * m_yx + m_zx * m_zx),
Sqrt(m_xy * m_xy + m_yy * m_yy + m_zy * m_zy),
Sqrt(m_xz * m_xz + m_yz * m_yz + m_zz * m_zz));
}
template<typename T>
CMat3x3<T> CMat3x3<T>::GetScaledRows(
const CVec3<T> &v) const
{
return CMat3x3<T>(
v.m_x * m_xx, v.m_x * m_xy, v.m_x * m_xz,
v.m_y * m_yx, v.m_y * m_yy, v.m_y * m_yz,
v.m_z * m_zx, v.m_z * m_zy, v.m_z * m_zz);
}
template<typename T>
CMat3x3<T> CMat3x3<T>::GetScaledCols(
const CVec3<T> &v) const
{
return CMat3x3<T>(
v.m_x * m_xx, v.m_y * m_xy, v.m_z * m_xz,
v.m_x * m_yx, v.m_y * m_yy, v.m_z * m_yz,
v.m_x * m_zx, v.m_y * m_zy, v.m_z * m_zz);
}
///////////////////////////////////////////////////////////////////////////////
template<typename T>
CMat3x3<T> CMat3x3<T>::ByTensorProd(
const CVec3<T> &v1, const CVec3<T> &v2)
{
return CMat3x3<T>(
v1.m_x * v2.m_x, v1.m_x * v2.m_y, v1.m_x * v2.m_z,
v1.m_y * v2.m_x, v1.m_y * v2.m_y, v1.m_y * v2.m_z,
v1.m_z * v2.m_x, v1.m_z * v2.m_y, v1.m_z * v2.m_z);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByDual(const CVec3<T> &v)
{
return CMat3x3<T>(
0, v.m_z, - v.m_y, - v.m_z, 0, v.m_x, v.m_y, - v.m_x, 0);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByDiag(const CVec3<T> &v)
{
return CMat3x3<T>(v.m_x, 0, 0, 0, v.m_y, 0, 0, 0, v.m_z);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByDiag(T x, T y, T z)
{
return CMat3x3<T>(x, 0, 0, 0, y, 0, 0, 0, z);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByRows(
const CVec3<T> &vx,
const CVec3<T> &vy,
const CVec3<T> &vz)
{
return CMat3x3<T>(
vx.m_x, vx.m_y, vx.m_z,
vy.m_x, vy.m_y, vy.m_z,
vz.m_x, vz.m_y, vz.m_z);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::ByCols(
const CVec3<T> &vx,
const CVec3<T> &vy,
const CVec3<T> &vz)
{
return CMat3x3<T>(
vx.m_x, vy.m_x, vz.m_x,
vx.m_y, vy.m_y, vz.m_y,
vx.m_z, vy.m_z, vz.m_z);
}
///////////////////////////////////////////////////////////////////////////////
template<typename T>
inline bool CMat3x3<T>::operator==(const CMat3x3<T> &m) const
{
return m_xx == m.m_xx && m_xy == m.m_xy && m_xz == m.m_xz
&& m_yx == m.m_yx && m_yy == m.m_yy && m_yz == m.m_yz
&& m_zx == m.m_zx && m_zy == m.m_zy && m_zz == m.m_zz;
}
template<typename T>
inline bool CMat3x3<T>::operator!=(const CMat3x3<T> &m) const
{
return m_xx != m.m_xx || m_xy != m.m_xy || m_xz != m.m_xz
|| m_yx != m.m_yx || m_yy != m.m_yy || m_yz != m.m_yz
|| m_zx != m.m_zx || m_zy != m.m_zy || m_zz != m.m_zz;
}
template<typename T>
int CMat3x3<T>::CompareComp(const CMat3x3<T> &m) const
{
if(m_xx < m.m_xx)
return -1;
else if(m_xx > m.m_xx)
return 1;
else if(m_xy < m.m_xy)
return -1;
else if(m_xy > m.m_xy)
return 1;
else if(m_xz < m.m_xz)
return -1;
else if(m_xz > m.m_xz)
return 1;
else if(m_yx < m.m_yx)
return -1;
else if(m_yx > m.m_yx)
return 1;
else if(m_yy < m.m_yy)
return -1;
else if(m_yy > m.m_yy)
return 1;
else if(m_yz < m.m_yy)
return -1;
else if(m_yz > m.m_yz)
return 1;
else if(m_zx < m.m_zx)
return -1;
else if(m_zx > m.m_zx)
return 1;
else if(m_zy < m.m_zy)
return -1;
else if(m_zy > m.m_zy)
return 1;
else if(m_zz < m.m_zy)
return -1;
else if(m_zz > m.m_zz)
return 1;
else
return 0;
}
///////////////////////////////////////////////////////////////////////////////
template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator+(const CMat3x3<T> &m) const
{
return CMat3x3<T>(
m_xx + m.m_xx, m_xy + m.m_xy, m_xz + m.m_xz,
m_yx + m.m_yx, m_yy + m.m_yy, m_yz + m.m_yz,
m_zx + m.m_zx, m_zy + m.m_zy, m_zz + m.m_zz);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator-(const CMat3x3<T> &m) const
{
return CMat3x3<T>(
m_xx - m.m_xx, m_xy - m.m_xy, m_xz - m.m_xz,
m_yx - m.m_yx, m_yy - m.m_yy, m_yz - m.m_yz,
m_zx - m.m_zx, m_zy - m.m_zy, m_zz - m.m_zz);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator*(T s) const
{
return CMat3x3<T>(
m_xx * s, m_xy * s, m_xz * s,
m_yx * s, m_yy * s, m_yz * s,
m_zx * s, m_zy * s, m_zz * s);
}
template<typename T>
inline CMat3x3<T> CMat3x3<T>::operator/(T s) const
{
return CMat3x3<T>(
m_xx / s, m_xy / s, m_xz / s,
m_yx / s, m_yy / s, m_yz / s,
m_zx / s, m_zy / s, m_zz / s);
}
template<typename T>
CMat3x3<T> CMat3x3<T>::operator*(const CMat3x3<T> &m) const
{
return CMat3x3<T>(
m_xx * m.m_xx + m_xy * m.m_yx + m_xz * m.m_zx,
m_xx * m.m_xy + m_xy * m.m_yy + m_xz * m.m_zy,
m_xx * m.m_xz + m_xy * m.m_yz + m_xz * m.m_zz,
m_yx * m.m_xx + m_yy * m.m_yx + m_yz * m.m_zx,
m_yx * m.m_xy + m_yy * m.m_yy + m_yz * m.m_zy,
m_yx * m.m_xz + m_yy * m.m_yz + m_yz * m.m_zz,
m_zx * m.m_xx + m_zy * m.m_yx + m_zz * m.m_zx,
m_zx * m.m_xy + m_zy * m.m_yy + m_zz * m.m_zy,
m_zx * m.m_xz + m_zy * m.m_yz + m_zz * m.m_zz);
}
template<typename T>
CVec3<T> CMat3x3<T>::operator*(const CVec3<T> &v) const
{
return CVec3<T>(
m_xx * v.m_x + m_xy * v.m_y + m_xz * v.m_z,
m_yx * v.m_x + m_yy * v.m_y + m_yz * v.m_z,
m_zx * v.m_x + m_zy * v.m_y + m_zz * v.m_z);
}
/*
* CQuat Implementation
*/
template<typename T>
inline CQuat<T>::CQuat()
{
}
template<typename T>
inline CQuat<T>::CQuat(T s)
{
m_t = s;
m_x = m_y = m_z = 0;
}
template<typename T>
inline CQuat<T>::CQuat(T t, T x, T y, T z)
{
m_t = t;
m_x = x;
m_y = y;
m_z = z;
}
template<typename T>
inline CQuat<T>::CQuat(T t, const CVec3<T> &v)
{
m_t = t;
m_x = v.m_x;
m_y = v.m_y;
m_z = v.m_z;
}
template<typename T>
inline CQuat<T>::CQuat(const CQuat<T> &q)
{
m_t = q.m_t;
m_x = q.m_x;
m_y = q.m_y;
m_z = q.m_z;
}
template<typename T>
inline CVec3<T> CQuat<T>::GetVec() const
{
return CVec3<T>(m_x, m_y, m_z);
}
template<typename T>
inline void CQuat<T>::SetVec(const CVec3<T> &v)
{
m_x = v.m_x;
m_y = v.m_y;
m_z = v.m_z;
}
template<typename T>
inline CQuat<T> &CQuat<T>::operator=(const CQuat<T> &q)
{
m_t = q.m_t;
m_x = q.m_x;
m_y = q.m_y;
m_z = q.m_z;
return (*this);
}
template<typename T>
inline CQuat<T> CQuat<T>::operator+() const
{
return (*this);
}
template<typename T>
inline CQuat<T> CQuat<T>::operator-() const
{
return CQuat<T>(- m_t, - m_x, - m_y, - m_z);
}
template<typename T>
inline CQuat<T> &CQuat<T>::operator+=(const CQuat<T> &q)
{
return (*this) = (*this) + q;
}
template<typename T>
inline CQuat<T> &CQuat<T>::operator-=(const CQuat<T> &q)
{
return (*this) = (*this) - q;
}
template<typename T>
inline CQuat<T> &CQuat<T>::operator*=(const CQuat<T> &q)
{
return (*this) = (*this) * q;
}
template<typename T>
inline CQuat<T> &CQuat<T>::operator*=(T s)
{
return (*this) = (*this) * s;
}
template<typename T>
inline CQuat<T> &CQuat<T>::operator/=(T s)
{
return (*this) = (*this) / s;
}
template<typename T>
CQuat<T> operator*(const CMat3x3<T> &m, const CQuat<T> &q)
{
// = [q.t, m * q.v]
return CQuat<T>(
q.m_t,
m.m_xx * q.m_x + m.m_xy * q.m_y + m.m_xz * q.m_z,
m.m_yx * q.m_x + m.m_yy * q.m_y + m.m_yz * q.m_z,
m.m_zx * q.m_x + m.m_zy * q.m_y + m.m_zz * q.m_z);
}
template<typename T>
CQuat<T> operator*(const CQuat<T> &q, const CMat3x3<T> &m)
{
// = [q.t, q.v * m]
return CQuat<T>(
q.m_t,
q.m_x * m.m_xx + q.m_y * m.m_yx + q.m_z * m.m_zx,
q.m_x * m.m_xy + q.m_y * m.m_yy + q.m_z * m.m_zy,
q.m_x * m.m_xz + q.m_y * m.m_yz + q.m_z * m.m_zz);
}
template<typename T>
CVec3<T> operator*(const CVec3<T> &v, const CQuat<T> &q)
{
// = [v * q.t, v ^ q.v]
return CVec3<T>(
v.m_x * q.m_t + v.m_y * q.m_z - v.m_z * q.m_y,
v.m_y * q.m_t + v.m_z * q.m_x - v.m_x * q.m_z,
v.m_z * q.m_t + v.m_x * q.m_y - v.m_y * q.m_x);
}
template<typename T>
CVec3<T> operator*(const CQuat<T> &q, const CVec3<T> &v)
{
// = [q.t * v, q.v ^ v]
return CVec3<T>(
q.m_t * v.m_x + q.m_y * v.m_z - q.m_z * v.m_y,
q.m_t * v.m_y + q.m_z * v.m_x - q.m_x * v.m_z,
q.m_t * v.m_z + q.m_x * v.m_y - q.m_y * v.m_x);
}
template<typename T>
CQuat<T> CQuat<T>::GetAbs() const
{
if(m_t >= 0)
return CQuat<T>(m_t, m_x, m_y, m_z);
else
return CQuat<T>(- m_t, - m_x, - m_y, - m_z);
}
template<typename T>
inline CQuat<T> CQuat<T>::GetRev() const
{
return CQuat<T>(m_t, - m_x, - m_y, - m_z);
}
template<typename T>
CQuat<T> CQuat<T>::GetInv() const
{
T f = 1 / (m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
return CQuat<T>(f * m_t, - f * m_x, - f * m_y, - f * m_z);
}
template<typename T>
T CQuat<T>::GetSqLen() const
{
return m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z;
}
template<typename T>
T CQuat<T>::GetLen() const
{
return Sqrt(GetSqLen());
}
template<typename T>
inline T CQuat<T>::GetQuatAngle() const
{
return 0.5 * GetRotAngle();
}
template<typename T>
T CQuat<T>::GetRotAngle() const
{
T cc = m_t * m_t;
T ss = m_x * m_x + m_y * m_y + m_z * m_z;
return Acos((cc - ss) / (cc + ss));
}
template<typename T>
CQuat<T> CQuat<T>::GetNormalized() const
{
T f = 1 / Sqrt(m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
return CQuat<T>(f * m_t, f * m_x, f * m_y, f * m_z);
}
template<typename T>
CQuat<T> CQuat<T>::GetNormalizedTo(T Len) const
{
T f = Len / Sqrt(m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
return CQuat<T>(f * m_t, f * m_x, f * m_y, f * m_z);
}
template<typename T>
CQuat<T> CQuat<T>::GetSqRoot() const
{
T Len = Sqrt(m_t * m_t + m_x * m_x + m_y * m_y + m_z * m_z);
T RootArg = 2 * (Len + m_t);
if(RootArg < 0) // Maybe caused by precision errors.
RootArg = 0;
T Root = Sqrt(RootArg);
T f = 1 / Root;
if(!IsFinite(f))
f = 0;
return CQuat<T>(0.5 * Root, f * m_x, f * m_y, f * m_z);
}
template<typename T>
CQuat<T> CQuat<T>::NormalizedPowerTo(T Exponent) const
{
T cc = m_t * m_t;
T ss = m_x * m_x + m_y * m_y + m_z * m_z;
T dc = (cc - ss) / (cc + ss);
if(!IsFinite(dc))
{
// *this is near zero.
return CQuat<T>(1);
}
else
{
T Angle = 0.5 * Acos(dc);
T NewAngle = Exponent * Angle;
T hs = Sin(NewAngle);
T hc = Cos(NewAngle);
T f = hs / Sqrt(ss);
if(!IsFinite(f))
{
// *this is near to 1.
return CQuat<T>(1);
}
else
return CQuat<T>(hc, f * m_x, f * m_y, f * m_z);
}
}
template<typename T>
CQuat<T> CQuat<T>::PowerTo(T Exponent) const
{
T SqLen = m_t * m_t + + m_x * m_x + m_y * m_y + m_z * m_z;
T LogSqLen = Log(SqLen);
if(!IsFinite(LogSqLen))
return CQuat<T>(0); // *this was 0.
else
{
T NewLen = Exp(Exponent * 0.5f * LogSqLen);
return NormalizedPowerTo(Exponent) * NewLen;
}
}
template<typename T>
inline bool CQuat<T>::IsNull() const
{
return m_t == 0
&& m_x == 0
&& m_y == 0
&& m_z == 0;
}
template<typename T>
bool CQuat<T>::IsFinite() const
{
return IsFinite(m_t)
&& IsFinite(m_x)
&& IsFinite(m_y)
&& IsFinite(m_z);
}
template<typename T>
CMat3x3<T> CQuat<T>::GetMat() const
{
// M w = (t^2 - v^2) w + 2 v (v w) + 2 t (v ^ w)
T tt = m_t * m_t;
T xx = m_x * m_x;
T yy = m_y * m_y;
T zz = m_z * m_z;
T tx2 = 2 * m_t * m_x;
T ty2 = 2 * m_t * m_y;
T tz2 = 2 * m_t * m_z;
T xy2 = 2 * m_x * m_y;
T yz2 = 2 * m_y * m_z;
T zx2 = 2 * m_z * m_x;
return CMat3x3<T>(
tt + xx - yy - zz, xy2 - tz2, zx2 + ty2,
xy2 + tz2, tt + yy - zz - xx, yz2 - tx2,
zx2 - ty2, yz2 + tx2, tt + zz - xx - yy);
}
template<typename T>
inline CMat3x3<T> CQuat<T>::GetRevMat() const
{
return GetRev().GetMat();
}
template<typename T>
inline CMat3x3<T> CQuat<T>::GetInvMat() const
{
return GetInv().GetMat();
}
///////////////////////////////////////////////////////////////////////////////
template<typename T>
T CQuat<T>::ScalarProd(const CQuat<T> &q) const
{
return m_t * q.m_t + m_x * q.m_x + m_y * q.m_y + m_z * q.m_z;
}
template<typename T>
T CQuat<T>::GetQuatAngleBetween(const CQuat<T> &q) const
{
//CQuat<T> Quot = (GetInv() * q2);
CQuat<T> Quot = (GetInv() * q);
return Quot.GetQuatAngle();
}
template<typename T>
T CQuat<T>::GetRotAngleBetween(const CQuat<T> &q) const
{
//CQuat<T> Quot = (GetInv() * q2);
CQuat<T> Quot = (GetInv() * q);
return Quot.GetRotAngle();
}
template<typename T>
CQuat<T> CQuat<T>::InterpLinear(const CQuat<T> &q,
T Frac) const
{
const CQuat<T> &This = (*this);
return This + (q - This) * Frac;
}
template<typename T>
CQuat<T> CQuat<T>::InterpStrictExp(const CQuat<T> &q,
T Frac) const
{
const CQuat<T> qBase = (*this);
CQuat<T> Quot = (qBase.GetInv() * q);
CQuat<T> Delta = Quot.PowerTo(Frac);
return qBase * Delta;
}
template<typename T>
CQuat<T> CQuat<T>::InterpShortestExp(const CQuat<T> &q,
T Frac) const
{
const CQuat<T> &qBase = (*this);
CQuat<T> Quot = (qBase.GetInv() * q);
CQuat<T> Delta = Quot.GetAbs().PowerTo(Frac);
return qBase * Delta;
}
template<typename T>
CQuat<T> CQuat<T>::InterpStrictGeo(const CQuat<T> &q,
T Frac) const
{
const CQuat<T> qBase = (*this);
float Scale1 = qBase.GetSqLen();
float Scale2 = q.GetSqLen();
float Scale = Scale1 + (Scale2 - Scale1) * Frac;
float Fac = Sqrt(Scale / Scale1);
if(!IsFinite(Fac))
{
// *this is near zero.
return q * Fac;
}
else
{
CQuat<T> Diff = (qBase.GetRev() * q);
CQuat<T> Delta = Diff.NormalizedPowerTo(Frac);
// If q is near zero, Delta is 1.
return (qBase * Delta) * Fac;
}
}
template<typename T>
CQuat<T> CQuat<T>::InterpShortestGeo(const CQuat<T> &q,
T Frac) const
{
const CQuat<T> qBase = (*this);
float Scale1 = qBase.GetSqLen();
float Scale2 = q.GetSqLen();
float Scale = Scale1 + (Scale2 - Scale1) * Frac;
float Fac = Sqrt(Scale / Scale1);
if(!IsFinite(Fac))
{
// *this is near zero.
return q * Fac;
}
else
{
CQuat<T> Diff = (qBase.GetRev() * q);
CQuat<T> Delta = Diff.GetAbs().NormalizedPowerTo(Frac);
// If q is near zero, Delta is 1.
return (qBase * Delta) * Fac;
}
}
template<typename T>
CQuat<T> CQuat<T>::GetProjAlong(
const CVec3<T> &Dir) const
{
return CQuat<T>(m_t, GetVec().GetProjAlong(Dir));
}
template<typename T>
CQuat<T> CQuat<T>::GetProjOrthoLeft(
const CVec3<T> &Dir) const
{
return (*this) * GetProjAlong(Dir).GetInv();
}
template<typename T>
CQuat<T> CQuat<T>::GetProjOrthoRight(
const CVec3<T> &Dir) const
{
return GetProjAlong(Dir).GetInv() * (*this);
}
///////////////////////////////////////////////////////////////////////////////
template<typename T>
CQuat<T> CQuat<T>::ByRot(
const CVec3<T> &Axis, T Angle)
{
T HalfAngle = 0.5 * Angle;
T hs = Sin(HalfAngle);
T hc = Cos(HalfAngle);
T AxisLen = Axis.GetLen();
T f = hs / AxisLen;
if(ll3d::IsFinite(f))
return CQuat<T>(hc, f * Axis.m_x, f * Axis.m_y, f * Axis.m_z);
else
return CQuat<T>(1);
}
template<typename T>
CQuat<T> CQuat<T>::ByScaledRot(
const CVec3<T> &Axis, T Angle, T Scale)
{
T HalfAngle = 0.5 * Angle;
T hs = Sin(HalfAngle);
T hc = Cos(HalfAngle);
T AxisLen = Axis.GetLen();
T f = hs / AxisLen;
if(IsFinite(f))
return CQuat<T>(hc, f * Axis.m_x, f * Axis.m_y, f * Axis.m_z);
else
return CQuat<T>(AxisLen);
}
template<typename T>
CQuat<T> CQuat<T>::ByPathShortest(
const CVec3<T> &From,
const CVec3<T> &To)
{
CVec3<T> RezFrom = From.GetReziproc();
CQuat<T> Base(RezFrom * To, RezFrom ^ To);
return Base.GetSqRoot();
}
template<typename T>
CQuat<T> CQuat<T>::ByPathAxis(
const CVec3<T> &Axis,
const CVec3<T> &AlsoFrom,
const CVec3<T> &AlsoTo)
{
// Not using PathShortest(From x Axis, To x Axis)
// to avoid singularity at From = - To
// Technique:
// OrthoFrom := AlsoFrom.GetProjOrtho(Axis)
// OrthoTo := AlsoFrom.GetProjOrtho(Axis)
// sBase = sin(rotation_angle) = (OrthoFrom ^ OrthoTo) * UnitAxis
// cBase = cos(rotation_angle) = OrthoFrom * OrthoTo
// sHalf = sin(rotation_angle/2)
// cHalf = cos(rotation_angle/2)
CVec3<T> UnitAxis = Axis.GetNormalized();
T AxisFrom = UnitAxis * AlsoFrom;
T AxisTo = UnitAxis * AlsoTo;
T sBase = UnitAxis.Volume(AlsoFrom, AlsoTo);
T cBase = (AlsoFrom * AlsoTo) - AxisFrom * AxisTo;
T BaseLen = Sqrt(cBase * cBase + sBase * sBase);
T cUnit = cBase / BaseLen;
T sHalf = Sqrt(0.5f * Abs(1 - cUnit)); // Abs for protection
T cHalf = Sqrt(0.5f * Abs(1 + cUnit));
if(sBase < 0)
sHalf = - sHalf;
return CQuat<T>(cHalf, UnitAxis * sHalf);
}
template<typename T>
CQuat<T> CQuat<T>::ByPathAlso(
const CVec3<T> &From,
const CVec3<T> &To,
const CVec3<T> &AlsoFrom,
const CVec3<T> &AlsoTo)
{
// First calculate shortest rotation:
CQuat<T> Shortest = CQuat<T>::ByPathShortest(From, To);
CVec3<T> EffAlsoFrom = Shortest.GetMat() * AlsoFrom;
// Second, add extra rotation around To axis.
CQuat<T> Extra = CQuat<T>::ByPathAxis(To, EffAlsoFrom, AlsoTo);
return Extra * Shortest;
}
template<typename T>
CQuat<T> CQuat<T>::ByBestOfMat(
const CMat3x3<T> &m)
{
// It uses the formulars
// SqRoot(0.5 * (m.GetTrace() - s), v = - 0.5 * m.GetDual())
T s = Sqrt(m.GetSqLen() / 3);
T qt = 0.5 * (m.m_xx + m.m_yy + m.m_zz - s);
T qx = 0.5 * (m.m_zy - m.m_yz);
T qy = 0.5 * (m.m_xz - m.m_zx);
T qz = 0.5 * (m.m_yx - m.m_xy);
return CQuat<T>(qt, qx, qy, qz).GetSqRoot();
}
template<typename T>
CQuat<T> CQuat<T>::ByRotMat(
const CMat3x3<T> &m)
{
T s = Sqrt(m.GetSqLen() / 3);
T tt4 = s + m.m_xx + m.m_yy + m.m_zz;
T xx4 = (s + m.m_xx) - (m.m_yy + m.m_zz);
T yy4 = (s + m.m_yy) - (m.m_zz + m.m_xx);
T zz4 = (s + m.m_zz) - (m.m_xx + m.m_yy);
T t = tt4 > 0 ? 0.5 * Sqrt(tt4) : 0;
T x = xx4 > 0 ? (m.m_yz <= m.m_zy ? 0.5 : -0.5) * Sqrt(xx4) : 0;
T y = yy4 > 0 ? (m.m_zx <= m.m_xz ? 0.5 : -0.5) * Sqrt(yy4) : 0;
T z = zz4 > 0 ? (m.m_xy <= m.m_yx ? 0.5 : -0.5) * Sqrt(zz4) : 0;
return CQuat<T>(t, x, y, z);
}
template<typename T>
CQuat<T> CQuat<T>::ByRotXYZ(T Pan, T Tilt, T Roll)
{
T HalfPan = 0.5 * Pan;
T HalfTilt = 0.5 * Tilt;
T HalfRoll = 0.5 * Roll;
T SinX = Sin(HalfPan);
T CosX = Cos(HalfPan);
T SinY = Sin(HalfTilt);
T CosY = Cos(HalfTilt);
T SinZ = Sin(HalfRoll);
T CosZ = Cos(HalfRoll);
T SinXCosZ = SinX * CosZ;
T SinXSinZ = SinX * SinZ;
T CosXCosZ = CosX * CosZ;
T CosXSinZ = CosX * SinZ;
return CQuat<T>(
CosXCosZ * CosY - SinXSinZ * SinY,
SinXCosZ * CosY + CosXSinZ * SinY,
CosXCosZ * SinY - SinXSinZ * CosY,
CosXSinZ * CosY + SinXCosZ * SinY);
}
template<typename T>
void CQuat<T>::GetRotAngleXYZ(T &Pan, T &Tilt, T &Roll)
{
T SinX = 2 * (m_t * m_x - m_y * m_z);
T SinZ = 2 * (m_t * m_z - m_x * m_y);
T SinY = 2 * (m_t * m_y + m_z * m_x);
T CosU = m_t * m_t - m_y * m_y;
T CosV = m_z * m_z - m_x * m_x;
T CosX = CosU + CosV;
T CosZ = CosU - CosV;
T CosY = Sqrt(SinX * SinX + CosX * CosX);
Pan = Atan2(SinX, CosX);
Roll = Atan2(SinZ, CosZ);
Tilt = Atan2(SinY, CosY);
}
///////////////////////////////////////////////////////////////////////////////
template<typename T>
inline CQuat<T> CQuat<T>::operator+(const CQuat<T> &q) const
{
return CQuat<T>(m_t + q.m_t, m_x + q.m_x, m_y + q.m_y, m_z + q.m_z);
}
template<typename T>
inline CQuat<T> CQuat<T>::operator-(const CQuat<T> &q) const
{
return CQuat<T>(m_t - q.m_t, m_x - q.m_x, m_y - q.m_y, m_z - q.m_z);
}
template<typename T>
inline CQuat<T> CQuat<T>::operator*(T s) const
{
return CQuat<T>(m_t * s, m_x * s, m_y * s, m_z * s);
}
template<typename T>
inline CQuat<T> CQuat<T>::operator/(T s) const
{
return CQuat<T>(m_t / s, m_x / s, m_y / s, m_z / s);
}
template<typename T>
CQuat<T> CQuat<T>::operator*(const CQuat<T> &q) const
{
// == CQuat<T>(t1 * t2 - v1 * v2, t1 * v2 + v1 * t2 + v1 ^ v2);
return CQuat<T>(
m_t * q.m_t - m_x * q.m_x - m_y * q.m_y - m_z * q.m_z,
m_t * q.m_x + m_x * q.m_t + m_y * q.m_z - m_z * q.m_y,
m_t * q.m_y + m_y * q.m_t + m_z * q.m_x - m_x * q.m_z,
m_t * q.m_z + m_z * q.m_t + m_x * q.m_y - m_y * q.m_x);
}
template<typename T>
inline bool CQuat<T>::operator==(const CQuat<T> &q) const
{
return m_t == q.m_t && m_x == q.m_x && m_y == q.m_y && m_z == q.m_z;
}
template<typename T>
inline bool CQuat<T>::operator!=(const CQuat<T> &q) const
{
return m_t != q.m_t || m_x != q.m_x || m_y != q.m_y || m_z != q.m_z;
}
template<typename T>
int CQuat<T>::CompareComp(const CQuat<T> &q) const
{
if(m_t < q.m_t)
return -1;
else if(m_t > q.m_t)
return 1;
else if(m_x < q.m_x)
return -1;
else if(m_x > q.m_x)
return 1;
else if(m_y < q.m_y)
return -1;
else if(m_y > q.m_y)
return 1;
else if(m_z < q.m_z)
return -1;
else if(m_z > q.m_z)
return 1;
else
return 0;
}
}//namespace ll3d
#endif //_LL3D_VEC3_H