604 lines
15 KiB
C++
604 lines
15 KiB
C++
/****************************************************************************
|
||
Copyright (c) 2007, Radon Labs GmbH
|
||
Copyright (c) 2011-2013,WebJet Business Division,CYOU
|
||
|
||
http://www.genesis-3d.com.cn
|
||
|
||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
of this software and associated documentation files (the "Software"), to deal
|
||
in the Software without restriction, including without limitation the rights
|
||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
copies of the Software, and to permit persons to whom the Software is
|
||
furnished to do so, subject to the following conditions:
|
||
|
||
The above copyright notice and this permission notice shall be included in
|
||
all copies or substantial portions of the Software.
|
||
|
||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||
THE SOFTWARE.
|
||
****************************************************************************/
|
||
#pragma once
|
||
#include "core/types.h"
|
||
#include "math/scalar.h"
|
||
#include "math/float4.h"
|
||
#include "math/vector.h"
|
||
#include "math/OgreMath/OgreQuaternion.h"
|
||
|
||
//------------------------------------------------------------------------------
|
||
namespace Math
|
||
{
|
||
class quaternion;
|
||
|
||
// win32 VC++ does not support passing aligned objects as value so far
|
||
// here is a bug-report at microsoft saying so:
|
||
// http://connect.microsoft.com/VisualStudio/feedback/ViewFeedback.aspx?FeedbackID=334581
|
||
typedef const quaternion& __QuaternionArg;
|
||
|
||
class quaternion
|
||
{
|
||
public:
|
||
/// default constructor, NOTE: does NOT setup components!
|
||
quaternion();
|
||
/// construct from components
|
||
quaternion(scalar x, scalar y, scalar z, scalar w);
|
||
/// construct from float4
|
||
quaternion(float4 const &rhs);
|
||
/// copy constructor
|
||
/// !!!! copy constructor forbidden, otherwise passing point's to a function
|
||
/// !!!! via Registers doesnt work
|
||
//quaternion(const quaternion& rhs);
|
||
/// construct from Ogre::Quaternion
|
||
quaternion(Ogre::Quaternion rhs);
|
||
|
||
/// assignment operator
|
||
void operator=(const quaternion& rhs);
|
||
/// assign Ogre::Quaternion
|
||
void operator=(Ogre::Quaternion rhs);
|
||
/// equality operator
|
||
bool operator==(const quaternion& rhs) const;
|
||
/// inequality operator
|
||
bool operator!=(const quaternion& rhs) const;
|
||
// rotation of a vector by a quaternion
|
||
vector operator* (const vector& rkVector) const;
|
||
// plus operator
|
||
quaternion operator+ (const quaternion& rkQ) const;
|
||
// minus operator
|
||
quaternion operator- (const quaternion& rkQ) const;
|
||
|
||
/// load content from 16-byte-aligned memory
|
||
void load(const scalar* ptr);
|
||
/// stream content to 16-byte-aligned memory circumventing the write-cache
|
||
void stream(scalar* ptr) const;
|
||
|
||
/// set content
|
||
void set(scalar x, scalar y, scalar z, scalar w);
|
||
/// set from float4
|
||
void set(float4 const &f4);
|
||
/// set the x component
|
||
void set_x(scalar x);
|
||
/// set the y component
|
||
void set_y(scalar y);
|
||
/// set the z component
|
||
void set_z(scalar z);
|
||
/// set the w component
|
||
void set_w(scalar w);
|
||
|
||
/// read/write access to x component
|
||
scalar& x();
|
||
/// read/write access to y component
|
||
scalar& y();
|
||
/// read/write access to z component
|
||
scalar& z();
|
||
/// read/write access to w component
|
||
scalar& w();
|
||
/// read-only access to x component
|
||
scalar x() const;
|
||
/// read-only access to y component
|
||
scalar y() const;
|
||
/// read-only access to z component
|
||
scalar z() const;
|
||
/// read-only access to w component
|
||
scalar w() const;
|
||
|
||
/// return true if quaternion is identity
|
||
bool isidentity() const;
|
||
/// returns length
|
||
scalar length() const;
|
||
/// returns length squared
|
||
scalar lengthsq() const;
|
||
/// un-denormalize quaternion (this is sort of a hack since Maya likes to return denormal quaternions)
|
||
void undenormalize();
|
||
///dot product
|
||
scalar dot(const quaternion& q) const;
|
||
|
||
/// calculate the exponential
|
||
static quaternion exp(const quaternion& q0);
|
||
/// returns an identity quaternion
|
||
static quaternion identity();
|
||
/// conjugates quaternion
|
||
static quaternion conjugate(const quaternion& q);
|
||
/// conjugates and renormalizes quaternion
|
||
static quaternion inverse(const quaternion& q);
|
||
/// calculate the natural logarithm
|
||
static quaternion ln(const quaternion& q);
|
||
/// multiply 2 quaternions
|
||
static quaternion multiply(const quaternion& q0, const quaternion& q1);
|
||
/// compute unit length quaternion
|
||
static quaternion normalize(const quaternion& q);
|
||
/// build quaternion from yaw, pitch and roll
|
||
static quaternion rotationyawpitchroll(scalar yaw, scalar pitch, scalar roll);
|
||
/// interpolate between 2 quaternion using spherical interpolation
|
||
static quaternion slerp(const quaternion& q1, const quaternion& q2, scalar t);
|
||
/// convert quaternion to axis and angle
|
||
static void to_axisangle(const quaternion& q, float4& outAxis, scalar& outAngle);
|
||
/// rotate axis by angle
|
||
static quaternion rotationaxis(const float4& inAxis, const scalar& inAngle);
|
||
/// get yaw pitch roll in degree
|
||
static void getyawpitchroll(const quaternion& q, scalar& yaw, scalar& pitch, scalar& roll);
|
||
|
||
|
||
|
||
private:
|
||
friend class matrix44;
|
||
|
||
Ogre::Quaternion vec;
|
||
};
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline
|
||
quaternion::quaternion()
|
||
{
|
||
// empty
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline
|
||
quaternion::quaternion(scalar x, scalar y, scalar z, scalar w)
|
||
{
|
||
this->vec = Ogre::Quaternion(w,x,y,z);
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline
|
||
quaternion::quaternion(float4 const &rhs)
|
||
{
|
||
this->vec = Ogre::Quaternion(rhs.w(),rhs.x(),rhs.y(),rhs.z());
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline
|
||
quaternion::quaternion(Ogre::Quaternion rhs) :
|
||
vec(rhs)
|
||
{
|
||
// empty
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::operator=(const quaternion& rhs)
|
||
{
|
||
this->vec = rhs.vec;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::operator=(Ogre::Quaternion rhs)
|
||
{
|
||
this->vec = rhs;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline bool
|
||
quaternion::operator==(const quaternion& rhs) const
|
||
{
|
||
return this->vec == rhs.vec;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline bool
|
||
quaternion::operator!=(const quaternion& rhs) const
|
||
{
|
||
return this->vec != rhs.vec;
|
||
}
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::operator+( const quaternion& ths ) const
|
||
{
|
||
return quaternion( vec.x+ths.vec.x, vec.y+ths.vec.y, vec.z+ths.vec.z, vec.w+ths.vec.w);
|
||
}
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::operator-( const quaternion& ths ) const
|
||
{
|
||
return quaternion( vec.x-ths.vec.x, vec.y-ths.vec.y, vec.z-ths.vec.z, vec.w-ths.vec.w);
|
||
}
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::load(const scalar* ptr)
|
||
{
|
||
memcpy((void*)this->vec.ptr(),ptr,sizeof(quaternion));
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::stream(scalar* ptr) const
|
||
{
|
||
memcpy(ptr,(void*)this->vec.ptr(),sizeof(quaternion));
|
||
//XMStoreFloat4A((XMFLOAT4A*)ptr, this->vec);
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::set(scalar x, scalar y, scalar z, scalar w)
|
||
{
|
||
this->vec = Ogre::Quaternion(w,x,y,z);
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
inline void
|
||
quaternion::set_x(scalar x)
|
||
{
|
||
this->vec.x = x;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
inline void
|
||
quaternion::set_y(scalar y)
|
||
{
|
||
this->vec.y = y;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
inline void
|
||
quaternion::set_z(scalar z)
|
||
{
|
||
this->vec.z = z;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
inline void
|
||
quaternion::set_w(scalar w)
|
||
{
|
||
this->vec.w = w;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::set(float4 const &f4)
|
||
{
|
||
this->vec = quaternion(f4).vec;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar&
|
||
quaternion::x()
|
||
{
|
||
return this->vec.x;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar
|
||
quaternion::x() const
|
||
{
|
||
return this->vec.x;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar&
|
||
quaternion::y()
|
||
{
|
||
return this->vec.y;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar
|
||
quaternion::y() const
|
||
{
|
||
return this->vec.y;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar&
|
||
quaternion::z()
|
||
{
|
||
return this->vec.z;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar
|
||
quaternion::z() const
|
||
{
|
||
return this->vec.z;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar&
|
||
quaternion::w()
|
||
{
|
||
return this->vec.w;
|
||
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar
|
||
quaternion::w() const
|
||
{
|
||
return this->vec.w;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline bool
|
||
quaternion::isidentity() const
|
||
{
|
||
return this->vec == Ogre::Quaternion::IDENTITY;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar
|
||
quaternion::length() const
|
||
{
|
||
return float4(this->vec.x,this->vec.y,this->vec.z,this->vec.w).length();
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline scalar
|
||
quaternion::lengthsq() const
|
||
{
|
||
return float4(this->vec.x,this->vec.y,this->vec.z,this->vec.w).lengthsq();
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::undenormalize()
|
||
{
|
||
// nothing to do on the xbox, since denormal numbers are not supported by the vmx unit,
|
||
// it is being set to zero anyway
|
||
|
||
this->set_x(n_undenormalize(this->x()));
|
||
this->set_y(n_undenormalize(this->y()));
|
||
this->set_z(n_undenormalize(this->z()));
|
||
this->set_w(n_undenormalize(this->w()));
|
||
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::exp(const quaternion& q)
|
||
{
|
||
return quaternion(q.vec.Exp());
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::identity()
|
||
{
|
||
|
||
return quaternion(Ogre::Quaternion::IDENTITY);
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::conjugate(const quaternion& q)
|
||
{
|
||
return quaternion(-q.vec.x, -q.vec.y, -q.vec.z, q.vec.w);
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
|
||
__forceinline quaternion
|
||
quaternion::inverse(const quaternion& q)
|
||
{
|
||
quaternion ret = q;
|
||
ret.vec.Inverse();
|
||
return ret;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::ln(const quaternion& q)
|
||
{
|
||
quaternion ret = q;
|
||
ret.vec.Log();
|
||
return ret;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::multiply(const quaternion& q0, const quaternion& q1)
|
||
{
|
||
return quaternion(q0.vec * q1.vec);
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::normalize(const quaternion& q)
|
||
{
|
||
quaternion ret = q;
|
||
ret.vec.normalise();
|
||
return ret;
|
||
}
|
||
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::rotationyawpitchroll(scalar yaw, scalar pitch, scalar roll)
|
||
{
|
||
//quaternion q;
|
||
//q.vec = q.vec.CalRollPitchYaw(roll,pitch,yaw);
|
||
//return q;
|
||
|
||
quaternion result;
|
||
float num9 = roll * 0.5f;
|
||
float num6 = n_sin(num9);
|
||
float num5 = n_cos(num9);
|
||
float num8 = pitch * 0.5f;
|
||
float num4 = n_sin(num8);
|
||
float num3 = (float)n_cos(num8);
|
||
float num7 = yaw * 0.5f;
|
||
float num2 = n_sin(num7);
|
||
float num = n_cos(num7);
|
||
result.x() = ((num * num4) * num5) + ((num2 * num3) * num6);
|
||
result.y() = ((num2 * num3) * num5) - ((num * num4) * num6);
|
||
result.z() = ((num * num3) * num6) - ((num2 * num4) * num5);
|
||
result.w() = ((num * num3) * num5) + ((num2 * num4) * num6);
|
||
|
||
return result;
|
||
}
|
||
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline quaternion
|
||
quaternion::slerp(const quaternion& q1, const quaternion& q2, scalar t)
|
||
{
|
||
return quaternion(Ogre::Quaternion::Slerp(t,q1.vec,q2.vec));
|
||
}
|
||
//------------------------------------------------------------------------------
|
||
/**
|
||
*/
|
||
__forceinline void
|
||
quaternion::to_axisangle(const quaternion& q, float4& outAxis, scalar& outAngle)
|
||
{
|
||
Ogre::Degree d;
|
||
Ogre::Vector3 outA;
|
||
q.vec.ToAngleAxis(d,outA);
|
||
outAngle = d.valueRadians();
|
||
outAxis.set(outA.x,outA.y,outA.z,0.0f);
|
||
}
|
||
|
||
__forceinline quaternion
|
||
quaternion::rotationaxis(const float4& inAxis, const scalar& inAngle)
|
||
{
|
||
Ogre::Quaternion q;
|
||
q.FromAngleAxis( Ogre::Radian(inAngle), Ogre::Vector3(inAxis.x(),inAxis.y(),inAxis.z()));
|
||
return q;
|
||
}
|
||
|
||
__forceinline void
|
||
quaternion::getyawpitchroll(const quaternion& q, scalar& yaw, scalar& pitch, scalar& roll)
|
||
{
|
||
|
||
// Ogre::Radian y,p,r;
|
||
//
|
||
// q.vec.GetYawPitchRoll(y, p, r);
|
||
//
|
||
// yaw = y.valueRadians();
|
||
// pitch = p.valueRadians();
|
||
// roll = r.valueRadians();
|
||
|
||
//<2F>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>
|
||
//[3D<33><44>ѧ<EFBFBD><D1A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϸ<EFBFBD><CFB7><EFBFBD><EFBFBD>].(3D.Math.Primer.for.Graphics.and.Game.Development).(<28><>)Fletcher.Dunn.ɨ<><C9A8><EFBFBD><EFBFBD>.pdf
|
||
//<2F>½<EFBFBD>10.6.6, <20><>173ҳ<33><D2B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>嵥10.6
|
||
|
||
|
||
// //<2F><><EFBFBD><EFBFBD> sin(pitch)
|
||
float sp = -2.0f * (q.y() * q.z() - q.w() * q.x());
|
||
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>
|
||
if(n_abs(sp) > 0.9999f)
|
||
{
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD>¿<EFBFBD>
|
||
pitch = 1.570796f * sp;
|
||
//<2F><><EFBFBD><EFBFBD> heading, bank<6E><6B><EFBFBD><EFBFBD>
|
||
yaw = n_atan2(-q.x() * q.z() + q.w() * q.y(), 0.5f - q.y() * q.y() - q.z() * q.z());
|
||
roll = 0.0f;
|
||
}
|
||
else
|
||
{
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
|
||
pitch = n_asin(sp);
|
||
yaw = n_atan2(q.x() * q.z() + q.w() * q.y(), 0.5f - q.x() * q.x() - q.y() * q.y());
|
||
roll = n_atan2(q.x() * q.y() + q.w() * q.z(), 0.5f - q.x() * q.x() - q.z() * q.z());
|
||
}
|
||
}
|
||
|
||
__forceinline scalar
|
||
quaternion::dot(const Math::quaternion &q) const
|
||
{
|
||
return vec.Dot(Ogre::Quaternion(q.w(),q.x(),q.y(),q.z()));
|
||
}
|
||
|
||
} // namespace Math
|
||
//------------------------------------------------------------------------------
|