浏览量:2,711

三维旋转

本篇文章谢绝转载,也禁止用于任何商业目的。

代码的下载地址为:https://github.com/twinklingstar20/orientation

本篇文章主要介绍三维空间下旋转的三种表示形式:四元数、矩阵和欧拉角,阐述了四元数、矩阵的数学原理和实现,在附录中描述了与四元数类和矩阵类相关的向量类的实现策略。三种旋转表示形式的优缺点对比,参见文章《欧拉角、四元数和矩阵的对比》。

特别说明:向量类、四元数类、矩阵类的实现,是由NVIDIA公司Physx物理引擎开源部分的源码修改而来的。

1. 四元数

四元数(Quaternion)是由爱尔兰数学家威廉•卢云•哈密顿在1843年发现的数学概念,在图形学中有重要的应用。在3D程序中,通常用四元数来计算3D物体的旋转角度,与矩阵相比,四元数更加高效,占用的储存空间更小,此外也更便于插值。

任意一个四元数可以表示为:

\[q = \left[ {\begin{array}{*{20}{c}}x&y&z&w\end{array}} \right] = xi + yj + zk + w \tag{1}\]

其中,\({{i}^{2}}={{j}^{2}}={{k}^{2}}=ijk=-1,ij=k,jk=i,ki=j\),注意\(ij=k\),但是\(ji\ne k\)。四元数的长度(模长)表示为\(\left\| q \right\|=\sqrt{{{x}^{2}}+{{y}^{2}}+{{z}^{2}}+{{w}^{2}}}\),通常将四元数规一化(Normalized),即:

\[q = \left[ {\begin{array}{*{20}{c}}{\frac{x}{{\left\| q \right\|}}}&{\frac{y}{{\left\| q \right\|}}}&{\frac{z}{{\left\| q \right\|}}}&{\frac{w}{{\left\| q \right\|}}}\end{array}} \right] \tag{2}\]

四元数本质上是复数(Complex Number),复数就存在一个共轭的概念。若两个复数的实部相等,虚部互为相反数,则称这两个复数互为共轭(Conjugate),\(q\)的共轭复数\({{q}^{*}}\)表示为:

\[{q^*} = \left[ {\begin{array}{*{20}{c}}{ - x}&{ - y}&{ - z}&w\end{array}} \right] \tag{3}\]

四元数间的运算,遵循复数间运算法则。

加减法:

\[{q_1} \pm {q_2} = \left[ {\begin{array}{*{20}{c}}{{x_1} \pm {x_2}}&{{y_1} \pm {y_2}}&{{z_1} \pm {z_2}}&{{w_1} \pm {w_2}}\end{array}} \right] \tag{4}\]

与系数的乘积:

\[\lambda q = \left[ {\begin{array}{*{20}{c}}{\lambda x}&{\lambda y}&{\lambda z}&{\lambda w}\end{array}} \right] \tag{5}\]

点积:

\[{q_1} \cdot {q_2} = {x_1}{x_2} + {y_1}{y_2} + {z_1}{z_2} + {w_1}{w_2} \tag{6}\]

四元数间的点积与向量点积相似,也可以计算四元数之间的角度差,即:

\[\cos \phi = \frac{{{x_1}{x_2} + {y_1}{y_2} + {z_1}{z_2} + {w_1}{w_2}}}{{\left\| {{q_1}} \right\| \cdot \left\| {{q_2}} \right\|}} \tag{7}\]

四元数间的乘积不遵守交换律,表示为:

\[{q_1}{q_2} = \left( {{x_1}i + {y_1}j + {z_1}k + {w_1}} \right)\left( {{x_2}i + {y_2}j + {z_2}k + {w_2}} \right) \tag{8}\]

等式展开得:

\[{q_1}{q_2} = ai + bj + ck + d \tag{9}\]

其中,\(\begin{array}{l}a = {x_1}{w_2} + {w_1}{x_2} + {y_1}{z_2} - {z_1}{y_2}\\b = {y_1}{w_2} + {w_1}{y_2} + {z_1}{x_2} - {x_1}{z_2}\\c = {z_1}{w_2} + {w_1}{z_2} + {x_1}{y_2} - {y_1}{x_2}\\d = {w_1}{w_2} - {x_1}{x_2} - {y_1}{y_2} - {z_1}{z_2}\end{array}\)

任意一个四元数可以分解为向量和标量两部分,即\(q = \left( {\begin{array}{*{20}{c}}{\overrightarrow v }&w\end{array}} \right)\),则两个四元数\({q_1} = \left( {\begin{array}{*{20}{c}}{\overrightarrow {{v_1}} }&{{w_1}}\end{array}} \right),{q_2} = \left( {\begin{array}{*{20}{c}}{\overrightarrow {{v_2}} }&{{w_2}}\end{array}} \right)\)的乘积有下列等式成立,具体的推导参见[2][3]:

\[{q_1}{q_2} = \left( {\begin{array}{*{20}{c}}{\overrightarrow {{v_1}} }&{{w_1}}\end{array}} \right)\left( {\begin{array}{*{20}{c}}{\overrightarrow {{v_2}} }&{{w_2}}\end{array}} \right) = \left( {\begin{array}{*{20}{c}}{{w_1}\overrightarrow {{v_2}} + {w_2}\overrightarrow {{v_1}} + \overrightarrow {{v_1}} \times \overrightarrow {{v_2}} }&{{w_1}{w_2} - \overrightarrow {{v_1}} \cdot \overrightarrow {{v_2}} }\end{array}} \right) \tag{10}\]

根据欧拉旋转定理,任何坐标系的相对定向,可以用四个数来规定。四元数原理的核心也在于:三维空间内,所有的旋转都可以用四个数来表示。通过四元数来计算旋转,它能减少所需的工作,并减小舍入误差。在电脑图形学里,四元数的插值计算是很简单的,这有非常重要的应用价值。

2016-10-03-1

图1. 绕着指定轴的旋转[1]

在三维空间上,物体绕着任意一条轴旋转\(\theta \)度,任意一个绕着指定点的旋转都可以表示成绕着经过该点的轴的旋转,如图1所示,考虑绕着点\(Q\)的旋转,任意一个点\(P\)经过旋转变换后,得到新的点\(P'\)。用四元数表示绕着轴\(\overrightarrow{u}\)旋转\(\theta \)角度的旋转:

\[\begin{array}{l}x = {u_x} \cdot \sin \left( {\theta /2} \right)\\y = {u_y} \cdot \sin \left( {\theta /2} \right)\\z = {u_z} \cdot \sin \left( {\theta /2} \right)\\w = \cos \left( {\theta /2} \right)\end{array} \tag{11}\]

其中,\(\overrightarrow{u}=\left( {{u}_{x}},{{u}_{y}},{{u}_{z}} \right),\left\| \overrightarrow{u} \right\|=1\)

相反,给定一个四元数\(q=\left[ x,y,z,w \right],\left\| q \right\|=1\),可以计算出它表示的旋转轴和角度,如下所示:

\[\begin{array}{l}\theta = 2 \cdot {\rm{acos}}\left( w \right)\\\overrightarrow u = \left( {\frac{x}{{\sqrt {1 - {w^2}} }},\frac{y}{{\sqrt {1 - {w^2}} }},\frac{z}{{\sqrt {1 - {w^2}} }}} \right)\end{array} \tag{12}\]

其中,若\(w=\pm 1\),则\(q\)表示绕着任意轴旋转\(0\)度。

设存在一个向量\(\overrightarrow{u}=\left( {{u}_{x}},{{u}_{y}},{{u}_{z}} \right)\)和一个四元数\(q = \left[ {\begin{array}{*{20}{c}}{\overrightarrow v }&w\end{array}} \right],\left\| q \right\| = 1,\overrightarrow v = \left( {\begin{array}{*{20}{c}}x&y&z\end{array}} \right)\),四元数可以表示一个旋转,把向量\(\overrightarrow{u}\)进行旋转变换,得到新向量\(\overrightarrow{{{u}_{1}}}\),可以表示为:

\[\overrightarrow {{u_1}} = q\left[ {\begin{array}{*{20}{c}}{\overrightarrow u }&0\end{array}} \right]{q^*} \tag{13}\]

把等式(10)代入等式(13),且由拉格朗日公式\(\vec{a}\times (\vec{b}\times \vec{c})=\vec{b}(\vec{a}\cdot \vec{c})-\vec{c}(\vec{a}\cdot \vec{b})\),上式可以简化为:

\[\overrightarrow {{u_1}} = \left( {2{w^2} - 1} \right)\overrightarrow u + 2w\left( {\overrightarrow v \times \overrightarrow u } \right) + 2\left( {\overrightarrow v \cdot \overrightarrow u } \right)\overrightarrow v \tag{14}\]

若对向量\(\overrightarrow{u}\)进行四元数\(q\)表示的旋转的逆变换,得到新向量\(\overrightarrow{{{u}_{2}}}\),可以表示为:

\[\overrightarrow {{u_2}} = {q^*}\left[ {\begin{array}{*{20}{c}}{\overrightarrow u }&0\end{array}} \right]q \tag{15}\]

可以简化为:

\[\overrightarrow {{u_2}} = \left( {2{w^2} - 1} \right)\overrightarrow u - 2w\left( {\overrightarrow v \times \overrightarrow u } \right) + 2\left( {\overrightarrow v \cdot \overrightarrow u } \right)\overrightarrow v \tag{16}\]

2016-10-03-2

图2. 线性插值

最后,介绍下四元数的球面插值。以线性插值为例,如图2所示,在两个向量\({{P}_{0}},{{P}_{1}}\)之间插值,得到等式\(P\left( t \right)={{P}_{0}}+\left( {{P}_{1}}-{{P}_{0}} \right)t,t\in \left[ 0,1 \right]\),线性插值存在一个缺点是,在以\({{P}_{0}}\)为半径的圆的曲线轨迹上不是恒速变化的。

2016-10-03-3

图3. 球面插值

四元数的球面插值(Spherical Linear Interpolation)仍然只用于表示旋转,它是关于单位四元数构成的球表面上的操作。如图3所示,作辅助线,经过点\(P\left( t \right)\)作一条平行于\(\overline{O{{P}_{0}}}\)的直线,\(\overline{O{{P}_{1}}}\)相交于点\(C\);经过点\({{P}_{0}}\)作一条直线垂直于\(\overline{O{{P}_{1}}}\),并交于点\(A\); 经过点\(P\left( t \right)\),作一条直线平行于\(\overline{A{{P}_{0}}}\),与\(\overline{O{{P}_{1}}}\)交于点\(B\)。易知,\(\Delta O{{P}_{0}}A\sim \Delta CP\left( t \right)B\),则易知\(\left\| {{P}_{0}} \right\|/\left\| \overline{CP\left( t \right)} \right\|=\left\| \overline{A{{P}_{0}}} \right\|/\left\| \overline{BP\left( t \right)} \right\|\),即有:

\[a\left\| {{P_0}} \right\| = \frac{{\sin \left( {\left( {1 - t} \right)\theta } \right)}}{{\sin \theta }}\left\| {{P_0}} \right\| \tag{17}\]

同理,可以计算出:

\[b\left\| {{P_1}} \right\| = \frac{{\sin \left( {t\theta } \right)}}{{\sin \theta }}\left\| {{P_1}} \right\| \tag{18}\]

四元数的球面插值可以表示为:

\[lerp\left( {{q_1},{q_2},t} \right) = \frac{{\sin \left( {\left( {1 - t} \right)\theta } \right)}}{{\sin \theta }}{q_1} + \frac{{\sin \left( {t\theta } \right)}}{{\sin \theta }}{q_2} \tag{19}\]

2. 旋转矩阵

在二维空间上,绕着原点,沿着逆时针方向旋转\(\theta \)角,可以用\(3\times 3\)的矩阵表示为

\[R(\theta ) = \left( {\begin{array}{*{20}{c}}{\cos \theta }&{ - \sin \theta }&0\\{\sin \theta }&{\cos \theta }&0\\0&0&1\end{array}} \right) \tag{20}\]

在三维空间上,绕着\(x\)轴、\(y\)轴、\(z\)轴的旋转分别用\(4\times 4\)的矩阵\({{R}_{x}}(\theta ),{{R}_{y}}(\theta ),{{R}_{z}}(\theta )\)表示,有

\[{R_x}(\theta ) = \left( {\begin{array}{*{20}{c}}1&0&0&0\\0&{\cos \theta }&{ - \sin \theta }&0\\0&{\sin \theta }&{\cos \theta }&0\\0&0&0&1\end{array}} \right),{R_y}(\theta ) = \left( {\begin{array}{*{20}{c}}{\cos \theta }&0&{\sin \theta }&0\\0&1&0&0\\{ - \sin \theta }&0&{\cos \theta }&0\\0&0&0&1\end{array}} \right),{R_z}(\theta ) = \left( {\begin{array}{*{20}{c}}{\cos \theta }&{ - \sin \theta }&0&0\\{\sin \theta }&{\cos \theta }&0&0\\0&0&1&0\\0&0&0&1\end{array}} \right) \tag{21}\]

其中,沿着轴相反的方向观察,\(\theta \)表示逆时针旋转的角度。

如果希望三维空间上的物体能绕着\(z\)轴旋转\(\theta \)度,旋转中心的齐次表示为\(p\),那么该如何进行变换呢[1]?由于采用矩阵形式的变换,可以用矩阵的乘积来叠加。首先,应该进行平移变换,使\(p\)点与原点重合,这一过程可以用\({{T}_{3}}\cdot {{p}^{T}}\)表示;然后,进行旋转变换\({{R}_{z}}(\theta )\);最后,再进行位移变换,返回至物体原先坐标。总的变换矩阵用\(H\)表示,则可以用\(H\cdot {{p}^{T}}\)表示变换后的点,\(H\)为:

\[H = {T_3}(t){R_z}(\theta ){T_3}( - t) \tag{22}\]

在三维空间上,物体绕着任意一条轴旋转\(\theta \)度可以用\(4\times 4\)的旋转矩阵\(R\)表示[1]。任意一个绕着指定点的旋转都可以表示成绕着经过该点的轴的旋转,如图1所示,考虑绕着点\(Q\)的旋转,任意一个点\(P\)经过旋转变换后,得到新的点\(P'\)。我们可以把这样的旋转分解为几个步骤:

  1. 把轴\(\vec{u}\)进行两次旋转变换,即\({{R}_{y}}({{\theta }_{1}})\)\({{R}_{z}}(-{{\theta }_{2}})\),使得轴\(\vec{u}\)\(x\)轴方向相同;
  2. 绕着\(x\)轴旋转\({{\theta }_{3}}\)度,即\({{R}_{x}}(\theta )\)
  3. 与第i步相反的旋转变换,使得轴\(\vec{u}\)恢复到最初方向。

组合上述5个旋转变换,总的变换可以表示为:

\[R(\theta ) = {R_y}( - {\theta _1}){R_z}({\theta _2}){R_x}(\theta ){R_z}( - {\theta _2}){R_y}({\theta _1}) \tag{23}\]

把等式(21)中的变换等式代入(23)中,可以得到总的变换等式:

\[R(\theta ) = \left( {\begin{array}{*{20}{c}}{c + (1 - c)u_x^2}&{(1 - c){u_y}{u_x} - s{u_z}}&{(1 - c){u_z}{u_x} + s{u_y}}&0\\{(1 - c){u_x}{u_y} + s{u_z}}&{c + (1 - c)u_y^2}&{(1 - c){u_z}{u_y} - s{u_x}}&0\\{(1 - c){u_x}{u_z} - s{u_y}}&{(1 - c){u_y}{u_z} + s{u_x}}&{c + (1 - c)u_z^2}&0\\0&0&0&1\end{array}} \right) \tag{24}\]

其中,\(c=\cos \theta ,s=\sin \theta ,\vec{u}=({{u}_{x}},{{u}_{y}},{{u}_{z}})\)

相反,若给定一个旋转矩阵\(R(\theta )\),我们也可以求出它是绕着哪条轴旋转多少度角?设:

\[R(\theta ) = \left( {\begin{array}{*{20}{c}}{{m_{11}}}&{{m_{12}}}&{{m_{13}}}&0\\{{m_{21}}}&{{m_{22}}}&{{m_{23}}}&0\\{{m_{31}}}&{{m_{32}}}&{{m_{33}}}&0\\0&0&0&1\end{array}} \right) \]

由等式(24),可以计算出旋转的角度为

\[\theta = \arccos \frac{{{m_{11}} + {m_{22}} + {m_{33}} - 1}}{2} \tag{25}\]

\(\vec{u}\)三个方向的分量为

\[\begin{array}{l}{u_x} = \frac{{{m_{32}} - {m_{23}}}}{{2\sin \theta }}\\{u_y} = \frac{{{m_{13}} - {m_{31}}}}{{2\sin \theta }}\\{u_z} = \frac{{{m_{21}} - {m_{12}}}}{{2\sin \theta }}\end{array} \tag{26}\]

旋转矩阵\(R\)有三个重要的性质:

  1. \({{R}^{-1}}={{R}^{T}}\);
  2. \(R\cdot \vec{u}=\vec{u}\);
  3. \(\left| R \right|=1\)

单位四元数\(q=xi+yj+zk+w,\left\| q \right\|=1\),可以用矩阵形式表示为:

\[\left[ {\begin{array}{*{20}{c}}{1 - 2({y^2} + {z^2})}&{2(xy - wz)}&{2(yw + xz)}\\{2(xy + wz)}&{1 - 2({x^2} + {z^2})}&{2(yz - wx)}\\{2(xz - wy)}&{2(wx + yz)}&{1 - 2({x^2} + {y^2})}\end{array}} \right] \tag{27}\]

3. 欧拉角

欧拉角是用于描述刚体在三维空间的朝向,它是相对于指定参考坐标系的旋转,一个刚体的朝向,依赖于参考坐标系,按一定顺序,做出的三个欧拉角的旋转而构成的。

2016-10-03-4

图4. 欧拉角表示[4]

如图4所示,设\(xyz\)轴作为参考系坐标轴,\(xy\)平面与 \(XY\)平面的交点线,用英文字母\(N\)表示,则欧拉角可以由下列三个按次序的角度来定义:

  1. \(\alpha \)\(x\)轴与交点线的夹角;
  2. \(\beta \)\(z\)轴与\(Z\)轴的夹角;
  3. \(\gamma \)是交点线与\(X\)轴的夹角。

参考

[1] F. Hill, and S. Kelley. Computer Graphics Using OpenGL, 3/E, Pearson, 2007.
[2] WIKIPEDIA. “Quaternions and spatial rotation.” website<https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>
[3] WIKIPEDIA. “Quaternions.” website< https://en.wikipedia.org/wiki/Quaternion>.
[4] WIKIPEDIA. “Euler angles.” website< https://en.wikipedia.org/wiki/Euler_angles>.
[5] Fletcher Dunn and Ian Parberry. 3D math primer for graphics and game development. CRC Press, 2015.

附录

Dunn&Parberry(2015)[5]描述了几种向量类的设计策略:

1. 向量类选择的基本数据类型

向量类中选择float作为成员变量的基本数据类型,在没有高精度的要求,不需要采用double类型,而选择float类型可以节约内存资源。

2. 运算符重载

实现中,重载的运算符包括单目运算符”-“,双目运算符”/”,”/=” (除以一个标量), ”*”, ”*=”(乘以一个标量,或者标量乘以向量),”+”,”-“,”+=”, ”-=”, ”|”(点积),”^”(叉积),比较运算符”<”,”==”,”!=”。

3. 尽可能多的运用const语句

放在函数外的const可以避免意外而改变成员变量。

使用const SrVector&的传参方式,避免了调用构造函数,是一种传址方式,同时避免对象的值被改变。而且如果函数不是内联的,传值方式比传址方式需要更多的堆栈空间和更长的参数压栈时间。

4. 不使用虚函数

自定义向量操作没有太大用处,向量类是严格要求速度的类;如果使用了虚函数,优化器通常不能产生成员函数的内联代码;如果使用虚函数,就需要指向虚表指针,向量类所占内存增加,一种简单的验证方法就是在一个类中加上virtual关键字,输出它的大小sizeof(SrVector3),去掉virtual关键字,再输出它的大小sizeof(SrVector3)。

5. 不使用privateprotect等屏蔽字

向量类不适合屏蔽字,3D向量的表达是非常直观的,存储三个数或者两个数,而没有维护屏蔽的接口的需要。

6. 优化

一个著名的编程原则是“95%的时间消耗在5%的代码上”,从另一方面说,为了提高执行速度,必需找到瓶颈并优化它;另一方条关于优化的名言是:“过早的优化是一切罪恶的根源”。这句话的含义是优化那些非瓶颈的代码,使代码复杂化,却没有得到相应的回报。反正总结一句话就是向量类越简单越好。

四元数的C++实现类如下所示:

/************************************************************************		
\file 	SrQuaternion.h
\link	www.twinklingstar.cn
\author Twinkling Star
\date	2013/11/22
****************************************************************************/
#ifndef SR_FOUNDATION_NSQUATERNION_H_
#define SR_FOUNDATION_NSQUATERNION_H_
#include "SrSimpleTypes.h"
#include "SrVector3.h"

/** \addtogroup foundation
  @{
*/

/**
\brief This is a quaternion class. For more information on quaternion mathematics
consult a mathematics source on complex numbers.
 
*/

class SrQuaternion
	{
	public:
	/**
	\brief Default constructor, does not do any initialization.
	*/
	SR_INLINE SrQuaternion();

	/**
	\brief Copy constructor.
	*/
	SR_INLINE SrQuaternion(const SrQuaternion&);

	/**
	\brief copies xyz elements from v, and scalar from w (defaults to 0).
	*/
	SR_INLINE SrQuaternion(const SrVector3& v, float w = 0);

	/**
	\brief creates from angle-axis representation.

	note that if Angle > 360 the resulting rotation is Angle mod 360.
	
	<b>Unit:</b> Degrees
	*/
	SR_INLINE SrQuaternion(const float angle, const SrVector3 & axis);

	/**
	\brief Creates from orientation matrix.

	\param[in] m Rotation matrix to extract quaternion from.
	*/
	SR_INLINE SrQuaternion(const class SrMatrix33 &m); /* defined in SrMatrix33.h */


	/**
	\brief Set the quaternion to the identity rotation.
	*/
	SR_INLINE void id();

	/**
	\brief Test if the quaternion is the identity rotation.
	*/
	SR_INLINE bool isIdentityRotation() const;

	//setting:

	/**
	\brief Set the members of the quaternion, in order WXYZ
	*/
	SR_INLINE void setWXYZ(float w, float x, float y, float z);

	/**
	\brief Set the members of the quaternion, in order XYZW
	*/
	SR_INLINE void setXYZW(float x, float y, float z, float w);

	/**
	\brief Set the members of the quaternion, in order WXYZ
	*/
	SR_INLINE void setWXYZ(const float *);

	/**
	\brief Set the members of the quaternion, in order XYZW
	*/
	SR_INLINE void setXYZW(const float *);

	SR_INLINE SrQuaternion& operator=  (const SrQuaternion&);

	/**
	\brief Implicitly extends vector by a 0 w element.
	*/
	SR_INLINE SrQuaternion& operator=  (const SrVector3&);

	SR_INLINE void setx(const float& d);
	SR_INLINE void sety(const float& d);
	SR_INLINE void setz(const float& d);
	SR_INLINE void setw(const float& d);

	SR_INLINE void getWXYZ(SrF32 *) const;
	SR_INLINE void getXYZW(SrF32 *) const;

	SR_INLINE void getWXYZ(SrF64 *) const;
	SR_INLINE void getXYZW(SrF64 *) const;

	/**
	\brief returns true if all elements are finite (not NAN or INF, etc.)
	*/
	SR_INLINE bool isFinite() const;

	/**
	\brief sets to the quat [0,0,0,1]
	*/
	SR_INLINE void zero();

	/**
	\brief creates a random unit quaternion.
	*/
	SR_INLINE void random();
	/**
	\brief creates from angle-axis representation.

	Note that if Angle > 360 the resulting rotation is Angle mod 360.
	
	<b>Unit:</b> Degrees
	*/
	SR_INLINE void fromAngleAxis(float angle, const SrVector3 & axis);

	/**
	\brief Creates from angle-axis representation.

	Axis must be normalized!
	
	<b>Unit:</b> Radians
	*/
	SR_INLINE void fromAngleAxisFast(float AngleRadians, const SrVector3 & axis);

	/**
	\brief Sets this to the opposite rotation of this.
	*/
	SR_INLINE void invert();

	/**
	\brief Fetches the Angle/axis given by the SrQuaternion.

	<b>Unit:</b> Degrees
	*/
	SR_INLINE void getAngleAxis(float& Angle, SrVector3 & axis) const;

	/**
	\brief Gets the angle between this quat and the identity quaternion.

	<b>Unit:</b> Degrees
	*/
	SR_INLINE float getAngle() const;

	/**
	\brief Gets the angle between this quat and the argument

	<b>Unit:</b> Degrees
	*/
	SR_INLINE float getAngle(const SrQuaternion &) const;

	/**
	\brief This is the squared 4D vector length, should be 1 for unit quaternions.
	*/
	SR_INLINE float magnitudeSquared() const;

	/**
	\brief returns the scalar product of this and other.
	*/
	SR_INLINE float dot(const SrQuaternion &other) const;

	//modifiers:
	/**
	\brief maps to the closest unit quaternion.
	*/
	SR_INLINE void normalize();

	/*
	\brief assigns its own conjugate to itself.

	\note for unit quaternions, this is the inverse.
	*/
	SR_INLINE void conjugate();

	/**
	this = a * b
	*/
	SR_INLINE void multiply(const SrQuaternion& a, const SrQuaternion& b);

	/**
	this = a * v
	v is interpreted as quat [xyz0]
	*/
	SR_INLINE void multiply(const SrQuaternion& a, const SrVector3& v);

	/**
	this = slerp(t, a, b)
	*/
	SR_INLINE void slerp(const float t, const SrQuaternion& a, const SrQuaternion& b);

	/**
	rotates passed vec by rot expressed by unit quaternion.  overwrites arg with the result.
	*/
	SR_INLINE void rotate(SrVector3 &) const;

	/**
	rotates passed vec by this (assumed unitary)
	*/
	SR_INLINE const SrVector3 rot(const SrVector3 &) const;

	/**
	inverse rotates passed vec by this (assumed unitary)
	*/
	SR_INLINE const SrVector3 invRot(const SrVector3 &) const;

	/**
	transform passed vec by this rotation (assumed unitary) and translation p
	*/
	SR_INLINE const SrVector3 transform(const SrVector3 &v, const SrVector3 &p) const;

	/**
	inverse rotates passed vec by this (assumed unitary)
	*/
	SR_INLINE const SrVector3 invTransform(const SrVector3 &v, const SrVector3 &p) const;


	/**
	rotates passed vec by opposite of rot expressed by unit quaternion.  overwrites arg with the result.
	*/
	SR_INLINE void inverseRotate(SrVector3 &) const;



	/**
	negates all the elements of the quat.  q and -q represent the same rotation.
	*/
	SR_INLINE void negate();
	SR_INLINE SrQuaternion operator -() const; 

	SR_INLINE SrQuaternion& operator*= (const SrQuaternion&);
	SR_INLINE SrQuaternion& operator+= (const SrQuaternion&);
	SR_INLINE SrQuaternion& operator-= (const SrQuaternion&);
	SR_INLINE SrQuaternion& operator*= (const float s);

	/** the quaternion elements */
    float x,y,z,w;

	/** quaternion multiplication */
	SR_INLINE SrQuaternion operator *(const SrQuaternion &) const; 

	/** quaternion addition */
	SR_INLINE SrQuaternion operator +(const SrQuaternion &) const; 

	/** quaternion subtraction */
	SR_INLINE SrQuaternion operator -(const SrQuaternion &) const; 

	/** quaternion conjugate */
	SR_INLINE SrQuaternion operator !() const; 

    /* 
	ops we decided not to implement:
	bool  operator== (const SrQuaternion&) const;
	SrVector3  operator^  (const SrQuaternion& r_h_s) const;//same as normal quat rot, but casts itself into a vector.  (doesn't compute w term)
	SrQuaternion  operator*  (const SrVector3& v) const;//implicitly extends vector by a 0 w element.
	SrQuaternion  operator*  (const float Scale) const;
	*/

	friend class SrMatrix33;
	private:
		SrQuaternion(float ix, float iy, float iz, float iw);
	};




SR_INLINE SrQuaternion::SrQuaternion()
	{
	//nothing
	}


SR_INLINE SrQuaternion::SrQuaternion(const SrQuaternion& q) : x(q.x), y(q.y), z(q.z), w(q.w)
	{
	}


SR_INLINE SrQuaternion::SrQuaternion(const SrVector3& v, float s)						// copy constructor, assumes w=0 
	{
	x = v.x;
	y = v.y;
	z = v.z;
	w = s;
	}


SR_INLINE SrQuaternion::SrQuaternion(const float angle, const SrVector3 & axis)				// creates a SrQuaternion from an Angle axis -- note that if Angle > 360 the resulting rotation is Angle mod 360
	{
	fromAngleAxis(angle,axis);
	}


SR_INLINE void SrQuaternion::id()
	{
	x = float(0);
	y = float(0);
	z = float(0);
	w = float(1);
	}

SR_INLINE bool SrQuaternion::isIdentityRotation() const
{
	return x==0 && y==0 && z==0 && fabsf(w)==1;
}


SR_INLINE void SrQuaternion::setWXYZ(float sw, float sx, float sy, float sz)
	{
	x = sx;
	y = sy;
	z = sz;
	w = sw;
	}


SR_INLINE void SrQuaternion::setXYZW(float sx, float sy, float sz, float sw)
	{
	x = sx;
	y = sy;
	z = sz;
	w = sw;
	}


SR_INLINE void SrQuaternion::setWXYZ(const float * d)
	{
	x = d[1];
	y = d[2];
	z = d[3];
	w = d[0];
	}


SR_INLINE void SrQuaternion::setXYZW(const float * d)
	{
	x = d[0];
	y = d[1];
	z = d[2];
	w = d[3];
	}


SR_INLINE void SrQuaternion::getWXYZ(SrF32 *d) const
	{
	d[1] = (SrF32)x;
	d[2] = (SrF32)y;
	d[3] = (SrF32)z;
	d[0] = (SrF32)w;
	}


SR_INLINE void SrQuaternion::getXYZW(SrF32 *d) const
	{
	d[0] = (SrF32)x;
	d[1] = (SrF32)y;
	d[2] = (SrF32)z;
	d[3] = (SrF32)w;
	}


SR_INLINE void SrQuaternion::getWXYZ(SrF64 *d) const
	{
	d[1] = (SrF64)x;
	d[2] = (SrF64)y;
	d[3] = (SrF64)z;
	d[0] = (SrF64)w;
	}


SR_INLINE void SrQuaternion::getXYZW(SrF64 *d) const
	{
	d[0] = (SrF64)x;
	d[1] = (SrF64)y;
	d[2] = (SrF64)z;
	d[3] = (SrF64)w;
	}

//const methods
 
SR_INLINE bool SrQuaternion::isFinite() const
	{
	return SrMath::isFinite(x) 
		&& SrMath::isFinite(y) 
		&& SrMath::isFinite(z)
		&& SrMath::isFinite(w);
	}



SR_INLINE void SrQuaternion::zero()
	{
	x = float(0.0);
	y = float(0.0);
	z = float(0.0);
	w = float(1.0);
	}


SR_INLINE void SrQuaternion::negate()
	{
	x = -x;
	y = -y;
	z = -z;
	w = -w;
	}

SR_INLINE SrQuaternion SrQuaternion::operator-() const
	{
	return SrQuaternion(-x,-y,-z,-w);
	}


SR_INLINE void SrQuaternion::random()
	{
	x = SrMath::rand(float(0.0),float(1.0));
	y = SrMath::rand(float(0.0),float(1.0));
	z = SrMath::rand(float(0.0),float(1.0));
	w = SrMath::rand(float(0.0),float(1.0));
	normalize();
	}


SR_INLINE void SrQuaternion::fromAngleAxis(float Angle, const SrVector3 & axis)			// set the SrQuaternion by Angle-axis (see AA constructor)
	{
	x = axis.x;
	y = axis.y;
	z = axis.z;

	// required: Normalize the axis

	const float i_length =  float(1.0) / SrMath::sqrt( x*x + y*y + z*z );
	
	x = x * i_length;
	y = y * i_length;
	z = z * i_length;

	// now make a clQuaternionernion out of it
	float Half = SrMath::degToRad(Angle * float(0.5));

	w = SrMath::cos(Half);//this used to be w/o deg to rad.
	const float sin_theta_over_two = SrMath::sin(Half);
	x = x * sin_theta_over_two;
	y = y * sin_theta_over_two;
	z = z * sin_theta_over_two;
	}

SR_INLINE void SrQuaternion::fromAngleAxisFast(float AngleRadians, const SrVector3 & axis)
	{
	float s;
	SrMath::sinCos(AngleRadians * 0.5f, s, w);
	x = axis.x * s;
	y = axis.y * s;
	z = axis.z * s;
	}

SR_INLINE void SrQuaternion::invert()
	{
	x = -x;
	y = -y;
	z = -z;
	}

SR_INLINE void SrQuaternion::setx(const float& d) 
	{ 
	x = d;
	}


SR_INLINE void SrQuaternion::sety(const float& d) 
	{ 
	y = d;
	}


SR_INLINE void SrQuaternion::setz(const float& d) 
	{ 
	z = d;
	}


SR_INLINE void SrQuaternion::setw(const float& d) 
	{ 
	w = d;
	}


SR_INLINE void SrQuaternion::getAngleAxis(float& angle, SrVector3 & axis) const
	{
	//return axis and angle of rotation of quaternion
    angle = SrMath::acos(w) * float(2.0);		//this is getAngle()
    float sa = SrMath::sqrt(float(1.0) - w*w);
	if (sa)
		{
		axis.set(x/sa,y/sa,z/sa);
		angle = SrMath::radToDeg(angle);
		}
	else
		axis.set(float(1.0),float(0.0),float(0.0));

	}



SR_INLINE float SrQuaternion::getAngle() const
	{
	return SrMath::acos(w) * float(2.0);
	}



SR_INLINE float SrQuaternion::getAngle(const SrQuaternion & q) const
	{
	return SrMath::acos(dot(q)) * float(2.0);
	}


SR_INLINE float SrQuaternion::magnitudeSquared() const

//modifiers:
	{
	return x*x + y*y + z*z + w*w;
	}


SR_INLINE float SrQuaternion::dot(const SrQuaternion &v) const
	{
	return x * v.x + y * v.y + z * v.z  + w * v.w;
	}


SR_INLINE void SrQuaternion::normalize()											// convert this SrQuaternion to a unit clQuaternionernion
	{
	const float mag = SrMath::sqrt(magnitudeSquared());
	if (mag)
		{
		const float imag = float(1.0) / mag;
		
		x *= imag;
		y *= imag;
		z *= imag;
		w *= imag;
		}
	}


SR_INLINE void SrQuaternion::conjugate()											// convert this SrQuaternion to a unit clQuaternionernion
	{
	x = -x;
	y = -y;
	z = -z;
	}


SR_INLINE void SrQuaternion::multiply(const SrQuaternion& left, const SrQuaternion& right)		// this = a * b
	{
	float a,b,c,d;

	a =left.w*right.w - left.x*right.x - left.y*right.y - left.z*right.z;
	b =left.w*right.x + right.w*left.x + left.y*right.z - right.y*left.z;
	c =left.w*right.y + right.w*left.y + left.z*right.x - right.z*left.x;
	d =left.w*right.z + right.w*left.z + left.x*right.y - right.x*left.y;

	w = a;
	x = b;
	y = c;
	z = d;
	}


SR_INLINE void SrQuaternion::multiply(const SrQuaternion& left, const SrVector3& right)		// this = a * b
	{
	float a,b,c,d;

	a = - left.x*right.x - left.y*right.y - left.z *right.z;
	b =   left.w*right.x + left.y*right.z - right.y*left.z;
	c =   left.w*right.y + left.z*right.x - right.z*left.x;
	d =   left.w*right.z + left.x*right.y - right.x*left.y;

	w = a;
	x = b;
	y = c;
	z = d;
	}

SR_INLINE void SrQuaternion::slerp(const float t, const SrQuaternion& left, const SrQuaternion& right) // this = slerp(t, a, b)
	{
	const float	quatEpsilon = (float(1.0e-8f));

	*this = left;

	float cosine = 
		x * right.x + 
		y * right.y + 
		z * right.z + 
		w * right.w;		//this is left.dot(right)

	float sign = float(1);
	if (cosine < 0)
		{
		cosine = - cosine;
		sign = float(-1);
		}

	float Sin = float(1) - cosine*cosine;

	if(Sin>=quatEpsilon*quatEpsilon)	
		{
		Sin = SrMath::sqrt(Sin);
		const float angle = SrMath::atan2(Sin, cosine);
		const float i_sin_angle = float(1) / Sin;



		float lower_weight = SrMath::sin(angle*(float(1)-t)) * i_sin_angle;
		float upper_weight = SrMath::sin(angle * t) * i_sin_angle * sign;

		w = (w * (lower_weight)) + (right.w * (upper_weight));
		x = (x * (lower_weight)) + (right.x * (upper_weight));
		y = (y * (lower_weight)) + (right.y * (upper_weight));
		z = (z * (lower_weight)) + (right.z * (upper_weight));
		}
	}


SR_INLINE void SrQuaternion::rotate(SrVector3 & v) const						//rotates passed vec by rot expressed by quaternion.  overwrites arg ith the result.
	{
	//float msq = float(1.0)/magnitudeSquared();	//assume unit quat!
	SrQuaternion myInverse;
	myInverse.x = -x;//*msq;
	myInverse.y = -y;//*msq;
	myInverse.z = -z;//*msq;
	myInverse.w =  w;//*msq;

	//v = ((*this) * v) ^ myInverse;

	SrQuaternion left;
	left.multiply(*this,v);
	v.x =left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z;
	v.y =left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x;
	v.z =left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y;
	}


SR_INLINE void SrQuaternion::inverseRotate(SrVector3 & v) const				//rotates passed vec by opposite of rot expressed by quaternion.  overwrites arg ith the result.
	{
	//float msq = float(1.0)/magnitudeSquared();	//assume unit quat!
	SrQuaternion myInverse;
	myInverse.x = -x;//*msq;
	myInverse.y = -y;//*msq;
	myInverse.z = -z;//*msq;
	myInverse.w =  w;//*msq;

	//v = (myInverse * v) ^ (*this);
	SrQuaternion left;
	left.multiply(myInverse,v);
	v.x =left.w*x + w*left.x + left.y*z - y*left.z;
	v.y =left.w*y + w*left.y + left.z*x - z*left.x;
	v.z =left.w*z + w*left.z + left.x*y - x*left.y;
	}


SR_INLINE SrQuaternion& SrQuaternion::operator=  (const SrQuaternion& q)
	{
	x = q.x;
	y = q.y;
	z = q.z;
	w = q.w;
	return *this;
	}

#if 0
SrQuaternion& SrQuaternion::operator=  (const SrVector3& v)		//implicitly extends vector by a 0 w element.
	{
	x = v.x;
	y = v.y;
	z = v.z;
	w = float(1.0);
	return *this;
	}
#endif

SR_INLINE SrQuaternion& SrQuaternion::operator*= (const SrQuaternion& q)
	{
	float xx[4]; //working Quaternion
	xx[0] = w*q.w - q.x*x - y*q.y - q.z*z;
	xx[1] = w*q.x + q.w*x + y*q.z - q.y*z;
	xx[2] = w*q.y + q.w*y + z*q.x - q.z*x;
	z=w*q.z + q.w*z + x*q.y - q.x*y;

	w = xx[0];
	x = xx[1];
	y = xx[2];
	return *this;
	}


SR_INLINE SrQuaternion& SrQuaternion::operator+= (const SrQuaternion& q)
	{
	x+=q.x;
	y+=q.y;
	z+=q.z;
	w+=q.w;
	return *this;
	}


SR_INLINE SrQuaternion& SrQuaternion::operator-= (const SrQuaternion& q)
	{
	x-=q.x;
	y-=q.y;
	z-=q.z;
	w-=q.w;
	return *this;
	}


SR_INLINE SrQuaternion& SrQuaternion::operator*= (const float s)
	{
	x*=s;
	y*=s;
	z*=s;
	w*=s;
	return *this;
	}

SR_INLINE SrQuaternion::SrQuaternion(float ix, float iy, float iz, float iw)
{
	x = ix;
	y = iy;
	z = iz;
	w = iw;
}

SR_INLINE SrQuaternion SrQuaternion::operator*(const SrQuaternion &q) const
{
	return SrQuaternion(w*q.x + q.w*x + y*q.z - q.y*z,
				  w*q.y + q.w*y + z*q.x - q.z*x,
				  w*q.z + q.w*z + x*q.y - q.x*y,
				  w*q.w - x*q.x - y*q.y - z*q.z);
}

SR_INLINE SrQuaternion SrQuaternion::operator+(const SrQuaternion &q) const
{
	return SrQuaternion(x+q.x,y+q.y,z+q.z,w+q.w);
}

SR_INLINE SrQuaternion SrQuaternion::operator-(const SrQuaternion &q) const
{
	return SrQuaternion(x-q.x,y-q.y,z-q.z,w-q.w);
}

SR_INLINE SrQuaternion SrQuaternion::operator!() const
{
	return SrQuaternion(-x,-y,-z,w);
}



SR_INLINE const SrVector3 SrQuaternion::rot(const SrVector3 &v) const
    {
	SrVector3 qv(x,y,z);

	return (v*(w*w-0.5f) + (qv^v)*w + qv*(qv|v))*2;
    }

SR_INLINE const SrVector3 SrQuaternion::invRot(const SrVector3 &v) const
    {
	SrVector3 qv(x,y,z);

	return (v*(w*w-0.5f) - (qv^v)*w + qv*(qv|v))*2;
    }



SR_INLINE const SrVector3 SrQuaternion::transform(const SrVector3 &v, const SrVector3 &p) const
    {
	return rot(v)+p;
    }

SR_INLINE const SrVector3 SrQuaternion::invTransform(const SrVector3 &v, const SrVector3 &p) const
    {
	return invRot(v-p);
    }

/** @} */
#endif

这里提供矩阵的实现的C++代码,涉及SrMatrix33和SrMatrix34两个类的实现代码:

/************************************************************************		
\file 	SrMatrix33.h
\link	www.twinklingstar.cn
\author Twinkling Star
\date	2013/11/22
****************************************************************************/
#ifndef SR_FOUNDATION_SRMATRIX33T_H_
#define SR_FOUNDATION_SRMATRIX33T_H_
/** \addtogroup foundation
  @{
*/

#include "SrVector3.h"
#include "SrQuaternion.h"
class Mat33DataType
{
public:
	struct S
	{

		float        _11, _12, _13;
		float        _21, _22, _23;
		float        _31, _32, _33;
	};
	union 
	{
		S s;
		float m[3][3];
	};
};

/**
\brief Identifies a special matrix. Can be passed to the #SrMatrix33 constructor.
*/
enum SrMatrixType
	{
	/**
	\brief Matrix of all zeros.
	*/
	SR_ZERO_MATRIX,

	/**
	\brief Identity matrix.
	*/
	SR_IDENTITY_MATRIX
	};


/**
\brief 3x3 Matrix Class.

 The idea of the matrix/vector classes is to partition them into two parts:
 One is the data structure which may have different formatting (3x3, 3x4, 4x4),
 row or column major.  The other is a template class which has all the operators
 but is storage format independent.

 This way it should be easier to change formats depending on what is faster/slower
 on a particular platform.

 Design issue: We use nameless struct/unions here.
 Design issue: this used to be implemented with a template.  This had no benefit
 but it added syntactic complexity.  Currently we just use a typedef and a preprocessor switch 
 to change between different memory layouts.

 The matrix math in this class is storage format (row/col major) independent as far
 as the user is concerned.
 When the user wants to get/set raw data, he needs to specify what order the data is
 coming in.  
 
*/
class SrMatrix33
	{
	public:
	SR_INLINE SrMatrix33();

	/**
	\param type Special matrix type to initialize with.

	@see SrMatrixType
	*/
	SR_INLINE SrMatrix33(SrMatrixType type);
	SR_INLINE SrMatrix33(const SrVector3 &row0, const SrVector3 &row1, const SrVector3 &row2);

	SR_INLINE SrMatrix33(const SrMatrix33&m);
	SR_INLINE SrMatrix33(const SrQuaternion &m);
	SR_INLINE ~SrMatrix33();
	SR_INLINE const SrMatrix33& operator=(const SrMatrix33 &src);

	// Access elements

	//low level data access, single or double precision, with eventual translation:
	//for dense 9 element data
	SR_INLINE void setRowMajor(const SrF32 *);
	SR_INLINE void setRowMajor(const SrF32 d[][3]);
	SR_INLINE void setColumnMajor(const SrF32 *);
	SR_INLINE void setColumnMajor(const SrF32 d[][3]);
	SR_INLINE void getRowMajor(SrF32 *) const;
	SR_INLINE void getRowMajor(SrF32 d[][3]) const;
	SR_INLINE void getColumnMajor(SrF32 *) const;
	SR_INLINE void getColumnMajor(SrF32 d[][3]) const;

	SR_INLINE void setRowMajor(const SrF64 *);
	SR_INLINE void setRowMajor(const SrF64 d[][3]);
	SR_INLINE void setColumnMajor(const SrF64 *);
	SR_INLINE void setColumnMajor(const SrF64 d[][3]);
	SR_INLINE void getRowMajor(SrF64 *) const;
	SR_INLINE void getRowMajor(SrF64 d[][3]) const;
	SR_INLINE void getColumnMajor(SrF64 *) const;
	SR_INLINE void getColumnMajor(SrF64 d[][3]) const;


	//for loose 4-padded data.
	SR_INLINE void setRowMajorStride4(const SrF32 *);
	SR_INLINE void setRowMajorStride4(const SrF32 d[][4]);
	SR_INLINE void setColumnMajorStride4(const SrF32 *);
	SR_INLINE void setColumnMajorStride4(const SrF32 d[][4]);
	SR_INLINE void getRowMajorStride4(SrF32 *) const;
	SR_INLINE void getRowMajorStride4(SrF32 d[][4]) const;
	SR_INLINE void getColumnMajorStride4(SrF32 *) const;
	SR_INLINE void getColumnMajorStride4(SrF32 d[][4]) const;

	SR_INLINE void setRowMajorStride4(const SrF64 *);
	SR_INLINE void setRowMajorStride4(const SrF64 d[][4]);
	SR_INLINE void setColumnMajorStride4(const SrF64 *);
	SR_INLINE void setColumnMajorStride4(const SrF64 d[][4]);
	SR_INLINE void getRowMajorStride4(SrF64 *) const;
	SR_INLINE void getRowMajorStride4(SrF64 d[][4]) const;
	SR_INLINE void getColumnMajorStride4(SrF64 *) const;
	SR_INLINE void getColumnMajorStride4(SrF64 d[][4]) const;


	SR_INLINE void setRow(int row, const SrVector3 &);
	SR_INLINE void setColumn(int col, const SrVector3 &);
	SR_INLINE void getRow(int row, SrVector3 &) const;
	SR_INLINE void getColumn(int col, SrVector3 &) const;

	SR_INLINE SrVector3 getRow(int row) const;
	SR_INLINE SrVector3 getColumn(int col) const;


	//element access:
    SR_INLINE float & operator()(int row, int col);
    SR_INLINE const float & operator() (int row, int col) const;

	/**
	\brief returns true for identity matrix
	*/
	SR_INLINE bool isIdentity() const;

	/**
	\brief returns true for zero matrix
	*/
	SR_INLINE bool isZero() const;

	/**
	\brief returns true if all elems are finite (not NAN or INF, etc.)
	*/
	SR_INLINE bool isFinite() const;

	//create special matrices:

	/**
	\brief sets this matrix to the zero matrix.
	*/
	SR_INLINE void zero();

	/**
	\brief sets this matrix to the identity matrix.
	*/
	SR_INLINE void id();

	/**
	\brief this = -this
	*/
	SR_INLINE void setNegative();

	/**
	\brief sets this matrix to the diagonal matrix.
	*/
	SR_INLINE void diagonal(const SrVector3 &vec);

	/**
	\brief Sets this matrix to the Star(Skew Symetric) matrix.

	So that star(v) * x = v.cross(x) .
	*/
	SR_INLINE void star(const SrVector3 &vec);


	SR_INLINE void fromQuat(const SrQuaternion &);
	SR_INLINE void toQuat(SrQuaternion &) const;

	//modifications:

	SR_INLINE const SrMatrix33 &operator +=(const SrMatrix33 &s);
	SR_INLINE const SrMatrix33 &operator -=(const SrMatrix33 &s);
	SR_INLINE const SrMatrix33 &operator *=(float s);
	SR_INLINE const SrMatrix33 &operator /=(float s);

	/*
	Gram-Schmidt orthogonalization to correct numerical drift, plus column normalization
	Caution: I believe the current implementation does not work right!
	*/
//	void orthonormalize();


	/**
	\brief returns determinant
	*/
	SR_INLINE float determinant() const;

	/**
	\brief assigns inverse to dest.
	
	Returns false if singular (i.e. if no inverse exists), setting dest to identity.
	*/
	SR_INLINE bool getInverse(SrMatrix33& dest) const;

	/**
	\brief this = transpose(other)

	this == other is OK.
	*/
	SR_INLINE void setTransposed(const SrMatrix33& other);

	/**
	\brief this = transpose(this)
	*/
	SR_INLINE void setTransposed();

	/**
	\brief this = this * [ d.x 0 0; 0 d.y 0; 0 0 d.z];
	*/
	SR_INLINE void multiplyDiagonal(const SrVector3 &d);

	/**
	\brief this = transpose(this) * [ d.x 0 0; 0 d.y 0; 0 0 d.z];
	*/
	SR_INLINE void multiplyDiagonalTranspose(const SrVector3 &d);

	/**
	\brief dst = this * [ d.x 0 0; 0 d.y 0; 0 0 d.z];
	*/
	SR_INLINE void multiplyDiagonal(const SrVector3 &d, SrMatrix33 &dst) const;

	/**
	\brief dst = transpose(this) * [ d.x 0 0; 0 d.y 0; 0 0 d.z];
	*/
	SR_INLINE void multiplyDiagonalTranspose(const SrVector3 &d, SrMatrix33 &dst) const;

	/**
	\brief dst = this * src
	*/
	SR_INLINE void multiply(const SrVector3 &src, SrVector3 &dst) const;
	/**
	\brief dst = transpose(this) * src
	*/
	SR_INLINE void multiplyByTranspose(const SrVector3 &src, SrVector3 &dst) const;

	/**
	\brief this = a + b
	*/
	SR_INLINE void  add(const SrMatrix33 & a, const SrMatrix33 & b);
	/***
	\brief this = a - b
	*/
	SR_INLINE void  subtract(const SrMatrix33 &a, const SrMatrix33 &b);
	/**
	\brief this = s * a;
	*/
	SR_INLINE void  multiply(float s,  const SrMatrix33 & a);
	/**
	\brief this = left * right
	*/
	SR_INLINE void multiply(const SrMatrix33& left, const SrMatrix33& right);
	/**
	\brief this = transpose(left) * right

	\note #multiplyByTranspose() is faster.
	*/
	SR_INLINE void multiplyTransposeLeft(const SrMatrix33& left, const SrMatrix33& right);
	/**
	\brief this = left * transpose(right)
	
	\note faster than #multiplyByTranspose().
	*/
	SR_INLINE void multiplyTransposeRight(const SrMatrix33& left, const SrMatrix33& right);

	/**
	\brief this = left * transpose(right)
	*/
	SR_INLINE void multiplyTransposeRight(const SrVector3 &left, const SrVector3 &right);

	/**
	\brief this = rotation matrix around X axis

	<b>Unit:</b> Radians
	*/
	SR_INLINE void rotX(float angle);

	/**
	\brief this = rotation matrix around Y axis

	<b>Unit:</b> Radians
	*/
	SR_INLINE void rotY(float angle);

	/**
	\brief this = rotation matrix around Z axis

	<b>Unit:</b> Radians
	*/
	SR_INLINE void rotZ(float angle);


	//overloaded multiply, and transposed-multiply ops:

	/**
	\brief returns transpose(this)*src
	*/
	SR_INLINE SrVector3 operator%  (const SrVector3 & src) const;
	/**
	\brief matrix vector product
	*/
	SR_INLINE SrVector3 operator*  (const SrVector3 & src) const;
	/**
	\brief matrix product
	*/
	SR_INLINE SrMatrix33&	operator*= (const SrMatrix33& mat);
	/**
	\brief matrix difference
	*/
	SR_INLINE SrMatrix33	operator-  (const SrMatrix33& mat)	const;
	/**
	\brief matrix addition
	*/
	SR_INLINE SrMatrix33	operator+  (const SrMatrix33& mat)	const;
	/**
	\brief matrix product
	*/
	SR_INLINE SrMatrix33	operator*  (const SrMatrix33& mat)	const;
	/**
	\brief matrix scalar product
	*/
	SR_INLINE SrMatrix33	operator*  (float s)				const;

	private:
	Mat33DataType data;
	};


SR_INLINE SrMatrix33::SrMatrix33()
	{
	}


SR_INLINE SrMatrix33::SrMatrix33(SrMatrixType type)
	{
		switch(type)
		{
			case SR_ZERO_MATRIX:		zero();	break;
			case SR_IDENTITY_MATRIX:	id();	break;
		}
	}


SR_INLINE SrMatrix33::SrMatrix33(const SrMatrix33& a)
	{
	data = a.data;
	}


SR_INLINE SrMatrix33::SrMatrix33(const SrQuaternion &q)
	{
	fromQuat(q);
	}

SR_INLINE SrMatrix33::SrMatrix33(const SrVector3 &row0, const SrVector3 &row1, const SrVector3 &row2)
{
	data.s._11 = row0.x;  data.s._12 = row0.y;  data.s._13 = row0.z;
	data.s._21 = row1.x;  data.s._22 = row1.y;  data.s._23 = row1.z;
	data.s._31 = row2.x;  data.s._32 = row2.y;  data.s._33 = row2.z;
}


SR_INLINE SrMatrix33::~SrMatrix33()
	{
	//nothing
	}


SR_INLINE const SrMatrix33& SrMatrix33::operator=(const SrMatrix33 &a)
	{
	data = a.data;
	return *this;
	}


SR_INLINE void SrMatrix33::setRowMajor(const SrF32* d)
	{
	//we are also row major, so this is a direct copy
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[1];
	data.s._13 = (float)d[2];

	data.s._21 = (float)d[3];
	data.s._22 = (float)d[4];
	data.s._23 = (float)d[5];

	data.s._31 = (float)d[6];
	data.s._32 = (float)d[7];
	data.s._33 = (float)d[8];
	}


SR_INLINE void SrMatrix33::setRowMajor(const SrF32 d[][3])
	{
	//we are also row major, so this is a direct copy
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[0][1];
	data.s._13 = (float)d[0][2];

	data.s._21 = (float)d[1][0];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[1][2];

	data.s._31 = (float)d[2][0];
	data.s._32 = (float)d[2][1];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::setColumnMajor(const SrF32* d)
	{
	//we are column major, so copy transposed.
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[3];
	data.s._13 = (float)d[6];

	data.s._21 = (float)d[1];
	data.s._22 = (float)d[4];
	data.s._23 = (float)d[7];

	data.s._31 = (float)d[2];
	data.s._32 = (float)d[5];
	data.s._33 = (float)d[8];
	}


SR_INLINE void SrMatrix33::setColumnMajor(const SrF32 d[][3])
	{
	//we are column major, so copy transposed.
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[1][0];
	data.s._13 = (float)d[2][0];

	data.s._21 = (float)d[0][1];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[2][1];

	data.s._31 = (float)d[0][2];
	data.s._32 = (float)d[1][2];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::getRowMajor(SrF32* d) const
	{
	//we are also row major, so this is a direct copy
	d[0] = (SrF32)data.s._11;
	d[1] = (SrF32)data.s._12;
	d[2] = (SrF32)data.s._13;

	d[3] = (SrF32)data.s._21;
	d[4] = (SrF32)data.s._22;
	d[5] = (SrF32)data.s._23;

	d[6] = (SrF32)data.s._31;
	d[7] = (SrF32)data.s._32;
	d[8] = (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::getRowMajor(SrF32 d[][3]) const
	{
	//we are also row major, so this is a direct copy
	d[0][0] = (SrF32)data.s._11;
	d[0][1] = (SrF32)data.s._12;
	d[0][2] = (SrF32)data.s._13;

	d[1][0] = (SrF32)data.s._21;
	d[1][1] = (SrF32)data.s._22;
	d[1][2] = (SrF32)data.s._23;

	d[2][0] = (SrF32)data.s._31;
	d[2][1] = (SrF32)data.s._32;
	d[2][2] = (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajor(SrF32* d) const
	{
	//we are column major, so copy transposed.
	d[0] = (SrF32)data.s._11;
	d[3] = (SrF32)data.s._12;
	d[6] = (SrF32)data.s._13;

	d[1] = (SrF32)data.s._21;
	d[4] = (SrF32)data.s._22;
	d[7] = (SrF32)data.s._23;

	d[2] = (SrF32)data.s._31;
	d[5] = (SrF32)data.s._32;
	d[8] = (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajor(SrF32 d[][3]) const
	{
	//we are column major, so copy transposed.
	d[0][0] = (SrF32)data.s._11;
	d[1][0] = (SrF32)data.s._12;
	d[2][0] = (SrF32)data.s._13;

	d[0][1] = (SrF32)data.s._21;
	d[1][1] = (SrF32)data.s._22;
	d[2][1] = (SrF32)data.s._23;

	d[0][2] = (SrF32)data.s._31;
	d[1][2] = (SrF32)data.s._32;
	d[2][2] = (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::setRowMajorStride4(const SrF32* d)
	{
	//we are also row major, so this is a direct copy
	//however we've got to skip every 4th element.
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[1];
	data.s._13 = (float)d[2];

	data.s._21 = (float)d[4];
	data.s._22 = (float)d[5];
	data.s._23 = (float)d[6];

	data.s._31 = (float)d[8];
	data.s._32 = (float)d[9];
	data.s._33 = (float)d[10];
	}


SR_INLINE void SrMatrix33::setRowMajorStride4(const SrF32 d[][4])
	{
	//we are also row major, so this is a direct copy
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[0][1];
	data.s._13 = (float)d[0][2];

	data.s._21 = (float)d[1][0];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[1][2];

	data.s._31 = (float)d[2][0];
	data.s._32 = (float)d[2][1];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::setColumnMajorStride4(const SrF32* d)
	{
	//we are column major, so copy transposed.
	//however we've got to skip every 4th element.
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[4];
	data.s._13 = (float)d[8];

	data.s._21 = (float)d[1];
	data.s._22 = (float)d[5];
	data.s._23 = (float)d[9];

	data.s._31 = (float)d[2];
	data.s._32 = (float)d[6];
	data.s._33 = (float)d[10];
	}


SR_INLINE void SrMatrix33::setColumnMajorStride4(const SrF32 d[][4])
	{
	//we are column major, so copy transposed.
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[1][0];
	data.s._13 = (float)d[2][0];

	data.s._21 = (float)d[0][1];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[2][1];

	data.s._31 = (float)d[0][2];
	data.s._32 = (float)d[1][2];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::getRowMajorStride4(SrF32* d) const
	{
	//we are also row major, so this is a direct copy
	//however we've got to skip every 4th element.
	d[0] = (SrF32)data.s._11;
	d[1] = (SrF32)data.s._12;
	d[2] = (SrF32)data.s._13;

	d[4] = (SrF32)data.s._21;
	d[5] = (SrF32)data.s._22;
	d[6] = (SrF32)data.s._23;

	d[8] = (SrF32)data.s._31;
	d[9] = (SrF32)data.s._32;
	d[10]= (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::getRowMajorStride4(SrF32 d[][4]) const
	{
	//we are also row major, so this is a direct copy
	d[0][0] = (SrF32)data.s._11;
	d[0][1] = (SrF32)data.s._12;
	d[0][2] = (SrF32)data.s._13;

	d[1][0] = (SrF32)data.s._21;
	d[1][1] = (SrF32)data.s._22;
	d[1][2] = (SrF32)data.s._23;

	d[2][0] = (SrF32)data.s._31;
	d[2][1] = (SrF32)data.s._32;
	d[2][2] = (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajorStride4(SrF32* d) const
	{
	//we are column major, so copy transposed.
	//however we've got to skip every 4th element.
	d[0] = (SrF32)data.s._11;
	d[4] = (SrF32)data.s._12;
	d[8] = (SrF32)data.s._13;

	d[1] = (SrF32)data.s._21;
	d[5] = (SrF32)data.s._22;
	d[9] = (SrF32)data.s._23;

	d[2] = (SrF32)data.s._31;
	d[6] = (SrF32)data.s._32;
	d[10]= (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajorStride4(SrF32 d[][4]) const
	{
	//we are column major, so copy transposed.
	d[0][0] = (SrF32)data.s._11;
	d[1][0] = (SrF32)data.s._12;
	d[2][0] = (SrF32)data.s._13;

	d[0][1] = (SrF32)data.s._21;
	d[1][1] = (SrF32)data.s._22;
	d[2][1] = (SrF32)data.s._23;

	d[0][2] = (SrF32)data.s._31;
	d[1][2] = (SrF32)data.s._32;
	d[2][2] = (SrF32)data.s._33;
	}


SR_INLINE void SrMatrix33::setRowMajor(const SrF64*d)
	{
	//we are also row major, so this is a direct copy
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[1];
	data.s._13 = (float)d[2];

	data.s._21 = (float)d[3];
	data.s._22 = (float)d[4];
	data.s._23 = (float)d[5];

	data.s._31 = (float)d[6];
	data.s._32 = (float)d[7];
	data.s._33 = (float)d[8];
	}


SR_INLINE void SrMatrix33::setRowMajor(const SrF64 d[][3])
	{
	//we are also row major, so this is a direct copy
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[0][1];
	data.s._13 = (float)d[0][2];

	data.s._21 = (float)d[1][0];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[1][2];

	data.s._31 = (float)d[2][0];
	data.s._32 = (float)d[2][1];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::setColumnMajor(const SrF64*d)
	{
	//we are column major, so copy transposed.
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[3];
	data.s._13 = (float)d[6];

	data.s._21 = (float)d[1];
	data.s._22 = (float)d[4];
	data.s._23 = (float)d[7];

	data.s._31 = (float)d[2];
	data.s._32 = (float)d[5];
	data.s._33 = (float)d[8];
	}


SR_INLINE void SrMatrix33::setColumnMajor(const SrF64 d[][3])
	{
	//we are column major, so copy transposed.
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[1][0];
	data.s._13 = (float)d[2][0];

	data.s._21 = (float)d[0][1];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[2][1];

	data.s._31 = (float)d[0][2];
	data.s._32 = (float)d[1][2];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::getRowMajor(SrF64*d) const
	{
	//we are also row major, so this is a direct copy
	d[0] = (SrF64)data.s._11;
	d[1] = (SrF64)data.s._12;
	d[2] = (SrF64)data.s._13;

	d[3] = (SrF64)data.s._21;
	d[4] = (SrF64)data.s._22;
	d[5] = (SrF64)data.s._23;

	d[6] = (SrF64)data.s._31;
	d[7] = (SrF64)data.s._32;
	d[8] = (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::getRowMajor(SrF64 d[][3]) const
	{
	//we are also row major, so this is a direct copy
	d[0][0] = (SrF64)data.s._11;
	d[0][1] = (SrF64)data.s._12;
	d[0][2] = (SrF64)data.s._13;

	d[1][0] = (SrF64)data.s._21;
	d[1][1] = (SrF64)data.s._22;
	d[1][2] = (SrF64)data.s._23;

	d[2][0] = (SrF64)data.s._31;
	d[2][1] = (SrF64)data.s._32;
	d[2][2] = (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajor(SrF64*d) const
	{
	//we are column major, so copy transposed.
	d[0] = (SrF64)data.s._11;
	d[3] = (SrF64)data.s._12;
	d[6] = (SrF64)data.s._13;

	d[1] = (SrF64)data.s._21;
	d[4] = (SrF64)data.s._22;
	d[7] = (SrF64)data.s._23;

	d[2] = (SrF64)data.s._31;
	d[5] = (SrF64)data.s._32;
	d[8] = (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajor(SrF64 d[][3]) const
	{
	//we are column major, so copy transposed.
	d[0][0] = (SrF64)data.s._11;
	d[1][0] = (SrF64)data.s._12;
	d[2][0] = (SrF64)data.s._13;

	d[0][1] = (SrF64)data.s._21;
	d[1][1] = (SrF64)data.s._22;
	d[2][1] = (SrF64)data.s._23;

	d[0][2] = (SrF64)data.s._31;
	d[1][2] = (SrF64)data.s._32;
	d[2][2] = (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::setRowMajorStride4(const SrF64*d)
	{
	//we are also row major, so this is a direct copy
	//however we've got to skip every 4th element.
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[1];
	data.s._13 = (float)d[2];

	data.s._21 = (float)d[4];
	data.s._22 = (float)d[5];
	data.s._23 = (float)d[6];

	data.s._31 = (float)d[8];
	data.s._32 = (float)d[9];
	data.s._33 = (float)d[10];
	}


SR_INLINE void SrMatrix33::setRowMajorStride4(const SrF64 d[][4])
	{
	//we are also row major, so this is a direct copy
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[0][1];
	data.s._13 = (float)d[0][2];

	data.s._21 = (float)d[1][0];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[1][2];

	data.s._31 = (float)d[2][0];
	data.s._32 = (float)d[2][1];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::setColumnMajorStride4(const SrF64*d)
	{
	//we are column major, so copy transposed.
	//however we've got to skip every 4th element.
	data.s._11 = (float)d[0];
	data.s._12 = (float)d[4];
	data.s._13 = (float)d[8];

	data.s._21 = (float)d[1];
	data.s._22 = (float)d[5];
	data.s._23 = (float)d[9];

	data.s._31 = (float)d[2];
	data.s._32 = (float)d[6];
	data.s._33 = (float)d[10];
	}


SR_INLINE void SrMatrix33::setColumnMajorStride4(const SrF64 d[][4])
	{
	//we are column major, so copy transposed.
	data.s._11 = (float)d[0][0];
	data.s._12 = (float)d[1][0];
	data.s._13 = (float)d[2][0];

	data.s._21 = (float)d[0][1];
	data.s._22 = (float)d[1][1];
	data.s._23 = (float)d[2][1];

	data.s._31 = (float)d[0][2];
	data.s._32 = (float)d[1][2];
	data.s._33 = (float)d[2][2];
	}


SR_INLINE void SrMatrix33::getRowMajorStride4(SrF64*d) const
	{
	//we are also row major, so this is a direct copy
	//however we've got to skip every 4th element.
	d[0] = (SrF64)data.s._11;
	d[1] = (SrF64)data.s._12;
	d[2] = (SrF64)data.s._13;

	d[4] = (SrF64)data.s._21;
	d[5] = (SrF64)data.s._22;
	d[6] = (SrF64)data.s._23;

	d[8] = (SrF64)data.s._31;
	d[9] = (SrF64)data.s._32;
	d[10]= (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::getRowMajorStride4(SrF64 d[][4]) const
	{
	//we are also row major, so this is a direct copy
	d[0][0] = (SrF64)data.s._11;
	d[0][1] = (SrF64)data.s._12;
	d[0][2] = (SrF64)data.s._13;

	d[1][0] = (SrF64)data.s._21;
	d[1][1] = (SrF64)data.s._22;
	d[1][2] = (SrF64)data.s._23;

	d[2][0] = (SrF64)data.s._31;
	d[2][1] = (SrF64)data.s._32;
	d[2][2] = (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajorStride4(SrF64*d) const

	{
	//we are column major, so copy transposed.
	//however we've got to skip every 4th element.
	d[0] = (SrF64)data.s._11;
	d[4] = (SrF64)data.s._12;
	d[8] = (SrF64)data.s._13;

	d[1] = (SrF64)data.s._21;
	d[5] = (SrF64)data.s._22;
	d[9] = (SrF64)data.s._23;

	d[2] = (SrF64)data.s._31;
	d[6] = (SrF64)data.s._32;
	d[10]= (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::getColumnMajorStride4(SrF64 d[][4]) const
	{
	//we are column major, so copy transposed.
	d[0][0] = (SrF64)data.s._11;
	d[1][0] = (SrF64)data.s._12;
	d[2][0] = (SrF64)data.s._13;

	d[0][1] = (SrF64)data.s._21;
	d[1][1] = (SrF64)data.s._22;
	d[2][1] = (SrF64)data.s._23;

	d[0][2] = (SrF64)data.s._31;
	d[1][2] = (SrF64)data.s._32;
	d[2][2] = (SrF64)data.s._33;
	}


SR_INLINE void SrMatrix33::setRow(int row, const SrVector3 & v)
	{
#ifndef TRANSPOSED_MAT33
	data.m[row][0] = v.x;
	data.m[row][1] = v.y;
	data.m[row][2] = v.z;
#else
	data.m[0][row] = v.x;
	data.m[1][row] = v.y;
	data.m[2][row] = v.z;
#endif
	}


SR_INLINE void SrMatrix33::setColumn(int col, const SrVector3 & v)
	{
#ifndef TRANSPOSED_MAT33
	data.m[0][col] = v.x;
	data.m[1][col] = v.y;
	data.m[2][col] = v.z;
#else
	data.m[col][0] = v.x;
	data.m[col][1] = v.y;
	data.m[col][2] = v.z;
#endif
	}


SR_INLINE void SrMatrix33::getRow(int row, SrVector3 & v) const
	{
#ifndef TRANSPOSED_MAT33
	v.x = data.m[row][0];
	v.y = data.m[row][1];
	v.z = data.m[row][2];
#else
	v.x = data.m[0][row];
	v.y = data.m[1][row];
	v.z = data.m[2][row];
#endif
	}


SR_INLINE void SrMatrix33::getColumn(int col, SrVector3 & v) const
	{
#ifndef TRANSPOSED_MAT33
	v.x = data.m[0][col];
	v.y = data.m[1][col];
	v.z = data.m[2][col];
#else
	v.x = data.m[col][0];
	v.y = data.m[col][1];
	v.z = data.m[col][2];
#endif
	}


SR_INLINE SrVector3 SrMatrix33::getRow(int row) const
{
#ifndef TRANSPOSED_MAT33
	return SrVector3(data.m[row][0],data.m[row][1],data.m[row][2]);
#else
	return SrVector3(data.m[0][row],data.m[1][row],data.m[2][row]);
#endif
}

SR_INLINE SrVector3 SrMatrix33::getColumn(int col) const
{
#ifndef TRANSPOSED_MAT33
	return SrVector3(data.m[0][col],data.m[1][col],data.m[2][col]);
#else
	return SrVector3(data.m[col][0],data.m[col][1],data.m[col][2]);
#endif
}

SR_INLINE float & SrMatrix33::operator()(int row, int col)
	{
#ifndef TRANSPOSED_MAT33
	return data.m[row][col];
#else
	return data.m[col][row];
#endif
	}


SR_INLINE const float & SrMatrix33::operator() (int row, int col) const
	{
#ifndef TRANSPOSED_MAT33
	return data.m[row][col];
#else
	return data.m[col][row];
#endif
	}

//const methods


SR_INLINE bool SrMatrix33::isIdentity() const
	{
	if(data.s._11 != 1.0f)		return false;
	if(data.s._12 != 0.0f)		return false;
	if(data.s._13 != 0.0f)		return false;

	if(data.s._21 != 0.0f)		return false;
	if(data.s._22 != 1.0f)		return false;
	if(data.s._23 != 0.0f)		return false;

	if(data.s._31 != 0.0f)		return false;
	if(data.s._32 != 0.0f)		return false;
	if(data.s._33 != 1.0f)		return false;

	return true;
	}


SR_INLINE bool SrMatrix33::isZero() const
	{
	if(data.s._11 != 0.0f)		return false;
	if(data.s._12 != 0.0f)		return false;
	if(data.s._13 != 0.0f)		return false;

	if(data.s._21 != 0.0f)		return false;
	if(data.s._22 != 0.0f)		return false;
	if(data.s._23 != 0.0f)		return false;

	if(data.s._31 != 0.0f)		return false;
	if(data.s._32 != 0.0f)		return false;
	if(data.s._33 != 0.0f)		return false;

	return true;
	}


SR_INLINE bool SrMatrix33::isFinite() const
	{
	return SrMath::isFinite(data.s._11)
	&& SrMath::isFinite(data.s._12)
	&& SrMath::isFinite(data.s._13)

	&& SrMath::isFinite(data.s._21)
	&& SrMath::isFinite(data.s._22)
	&& SrMath::isFinite(data.s._23)

	&& SrMath::isFinite(data.s._31)
	&& SrMath::isFinite(data.s._32)
	&& SrMath::isFinite(data.s._33);
	}



SR_INLINE void SrMatrix33::zero()
	{
	data.s._11 = float(0.0);
	data.s._12 = float(0.0);
	data.s._13 = float(0.0);

	data.s._21 = float(0.0);
	data.s._22 = float(0.0);
	data.s._23 = float(0.0);

	data.s._31 = float(0.0);
	data.s._32 = float(0.0);
	data.s._33 = float(0.0);
	}


SR_INLINE void SrMatrix33::id()
	{
	data.s._11 = float(1.0);
	data.s._12 = float(0.0);
	data.s._13 = float(0.0);

	data.s._21 = float(0.0);
	data.s._22 = float(1.0);
	data.s._23 = float(0.0);

	data.s._31 = float(0.0);
	data.s._32 = float(0.0);
	data.s._33 = float(1.0);
	}


SR_INLINE void SrMatrix33::setNegative()
	{
	data.s._11 = -data.s._11;
	data.s._12 = -data.s._12;
	data.s._13 = -data.s._13;

	data.s._21 = -data.s._21;
	data.s._22 = -data.s._22;
	data.s._23 = -data.s._23;

	data.s._31 = -data.s._31;
	data.s._32 = -data.s._32;
	data.s._33 = -data.s._33;
	}


SR_INLINE void SrMatrix33::diagonal(const SrVector3 &v)
	{
	data.s._11 = v.x;
	data.s._12 = float(0.0);
	data.s._13 = float(0.0);

	data.s._21 = float(0.0);
	data.s._22 = v.y;
	data.s._23 = float(0.0);

	data.s._31 = float(0.0);
	data.s._32 = float(0.0);
	data.s._33 = v.z;
	}


SR_INLINE void SrMatrix33::star(const SrVector3 &v)
	{
	data.s._11 = float(0.0);	data.s._12 =-v.z;	data.s._13 = v.y;
	data.s._21 = v.z;	data.s._22 = float(0.0);	data.s._23 =-v.x;
	data.s._31 =-v.y;	data.s._32 = v.x;	data.s._33 = float(0.0);
	}


SR_INLINE void SrMatrix33::fromQuat(const SrQuaternion & q)
	{
	const float w = q.w;
	const float x = q.x;
	const float y = q.y;
	const float z = q.z;

	data.s._11 = float(1.0) - y*y*float(2.0) - z*z*float(2.0);
	data.s._12 = x*y*float(2.0) - w*z*float(2.0);	
	data.s._13 = x*z*float(2.0) + w*y*float(2.0);	

	data.s._21 = x*y*float(2.0) + w*z*float(2.0);	
	data.s._22 = float(1.0) - x*x*float(2.0) - z*z*float(2.0);	
	data.s._23 = y*z*float(2.0) - w*x*float(2.0);	
	
	data.s._31 = x*z*float(2.0) - w*y*float(2.0);	
	data.s._32 = y*z*float(2.0) + w*x*float(2.0);	
	data.s._33 = float(1.0) - x*x*float(2.0) - y*y*float(2.0);	
	}


SR_INLINE void SrMatrix33::toQuat(SrQuaternion & q) const					// set the SrQuaternion from a rotation matrix
	{
    float tr, s;
    tr = data.s._11 + data.s._22 + data.s._33;
    if(tr >= 0)
		{
		s = (float)SrMath::sqrt(tr +1);
		q.w = float(0.5) * s;
		s = float(0.5) / s;
		q.x = ((*this)(2,1) - (*this)(1,2)) * s;
		q.y = ((*this)(0,2) - (*this)(2,0)) * s;
		q.z = ((*this)(1,0) - (*this)(0,1)) * s;
		}
    else
		{
		int i = 0; 
		if (data.s._22 > data.s._11)
			i = 1; 
		if(data.s._33 > (*this)(i,i))
			i=2; 
		switch (i)
			{
			case 0:
				s = (float)SrMath::sqrt((data.s._11 - (data.s._22 + data.s._33)) + 1);
				q.x = float(0.5) * s;
				s = float(0.5) / s;
				q.y = ((*this)(0,1) + (*this)(1,0)) * s; 
				q.z = ((*this)(2,0) + (*this)(0,2)) * s;
				q.w = ((*this)(2,1) - (*this)(1,2)) * s;
				break;
			case 1:
				s = (float)SrMath::sqrt((data.s._22 - (data.s._33 + data.s._11)) + 1);
				q.y = float(0.5) * s;
				s = float(0.5) / s;
				q.z = ((*this)(1,2) + (*this)(2,1)) * s;
				q.x = ((*this)(0,1) + (*this)(1,0)) * s;
				q.w = ((*this)(0,2) - (*this)(2,0)) * s;
				break;
			case 2:
				s = (float)SrMath::sqrt((data.s._33 - (data.s._11 + data.s._22)) + 1);
				q.z = float(0.5) * s;
				s = float(0.5) / s;
				q.x = ((*this)(2,0) + (*this)(0,2)) * s;
				q.y = ((*this)(1,2) + (*this)(2,1)) * s;
				q.w = ((*this)(1,0) - (*this)(0,1)) * s;
			}
		}
	}

SR_INLINE void SrMatrix33::setTransposed(const SrMatrix33& other)
	{
	//gotta special case in-place case
	if (this != &other)
		{
		data.s._11 = other.data.s._11;
		data.s._12 = other.data.s._21;
		data.s._13 = other.data.s._31;

		data.s._21 = other.data.s._12;
		data.s._22 = other.data.s._22;
		data.s._23 = other.data.s._32;

		data.s._31 = other.data.s._13;
		data.s._32 = other.data.s._23;
		data.s._33 = other.data.s._33;
		}
	else
		{
		float tx, ty, tz;
		tx = data.s._21;	data.s._21 = other.data.s._12;	data.s._12 = tx;
		ty = data.s._31;	data.s._31 = other.data.s._13;	data.s._13 = ty;
		tz = data.s._32;	data.s._32 = other.data.s._23;	data.s._23 = tz;
		}
	}


SR_INLINE void SrMatrix33::setTransposed()
	{
		float tempValue = data.s._21;data.s._21 = data.s._12; data.s._12 = tempValue;
		tempValue = data.s._23;data.s._23 = data.s._32; data.s._32 = tempValue;
		tempValue = data.s._13;data.s._13 = data.s._31; data.s._31 = tempValue;
	}


SR_INLINE void SrMatrix33::multiplyDiagonal(const SrVector3 &d)
	{
	data.s._11 *= d.x;
	data.s._12 *= d.y;
	data.s._13 *= d.z;

	data.s._21 *= d.x;
	data.s._22 *= d.y;
	data.s._23 *= d.z;

	data.s._31 *= d.x;
	data.s._32 *= d.y;
	data.s._33 *= d.z;
	}


SR_INLINE void SrMatrix33::multiplyDiagonalTranspose(const SrVector3 &d)
	{
		float temp;
		data.s._11 = data.s._11 * d.x;
		data.s._22 = data.s._22 * d.y;
		data.s._33 = data.s._33 * d.z;

		temp = data.s._21 * d.y;
		data.s._21 = data.s._12 * d.x;
		data.s._12 = temp;

		temp = data.s._31 * d.z;
		data.s._31 = data.s._13 * d.x;
		data.s._13 = temp;
		
		temp = data.s._32 * d.z;
		data.s._32 = data.s._23 * d.y;
		data.s._23 = temp;
	}


SR_INLINE void SrMatrix33::multiplyDiagonal(const SrVector3 &d, SrMatrix33& dst) const
	{
	dst.data.s._11 = data.s._11 * d.x;
	dst.data.s._12 = data.s._12 * d.y;
	dst.data.s._13 = data.s._13 * d.z;

	dst.data.s._21 = data.s._21 * d.x;
	dst.data.s._22 = data.s._22 * d.y;
	dst.data.s._23 = data.s._23 * d.z;

	dst.data.s._31 = data.s._31 * d.x;
	dst.data.s._32 = data.s._32 * d.y;
	dst.data.s._33 = data.s._33 * d.z;
	}


SR_INLINE void SrMatrix33::multiplyDiagonalTranspose(const SrVector3 &d, SrMatrix33& dst) const
	{
	dst.data.s._11 = data.s._11 * d.x;
	dst.data.s._12 = data.s._21 * d.y;
	dst.data.s._13 = data.s._31 * d.z;

	dst.data.s._21 = data.s._12 * d.x;
	dst.data.s._22 = data.s._22 * d.y;
	dst.data.s._23 = data.s._32 * d.z;

	dst.data.s._31 = data.s._13 * d.x;
	dst.data.s._32 = data.s._23 * d.y;
	dst.data.s._33 = data.s._33 * d.z;
	}


SR_INLINE void SrMatrix33::multiply(const SrVector3 &src, SrVector3 &dst) const
	{
	float x,y,z;	//so it works if src == dst
	x = data.s._11 * src.x + data.s._12 * src.y + data.s._13 * src.z;
	y = data.s._21 * src.x + data.s._22 * src.y + data.s._23 * src.z;
	z = data.s._31 * src.x + data.s._32 * src.y + data.s._33 * src.z;

	dst.x = x;
	dst.y = y;
	dst.z = z;	
	}


SR_INLINE void SrMatrix33::multiplyByTranspose(const SrVector3 &src, SrVector3 &dst) const
	{
	float x,y,z;	//so it works if src == dst
	x = data.s._11 * src.x + data.s._21 * src.y + data.s._31 * src.z;
	y = data.s._12 * src.x + data.s._22 * src.y + data.s._32 * src.z;
	z = data.s._13 * src.x + data.s._23 * src.y + data.s._33 * src.z;

	dst.x = x;
	dst.y = y;
	dst.z = z;	
	}


SR_INLINE void SrMatrix33::add(const SrMatrix33 & a, const SrMatrix33 & b)
	{
	data.s._11 = a.data.s._11 + b.data.s._11;
	data.s._12 = a.data.s._12 + b.data.s._12;
	data.s._13 = a.data.s._13 + b.data.s._13;

	data.s._21 = a.data.s._21 + b.data.s._21;
	data.s._22 = a.data.s._22 + b.data.s._22;
	data.s._23 = a.data.s._23 + b.data.s._23;

	data.s._31 = a.data.s._31 + b.data.s._31;
	data.s._32 = a.data.s._32 + b.data.s._32;
	data.s._33 = a.data.s._33 + b.data.s._33;
	}


SR_INLINE void SrMatrix33::subtract(const SrMatrix33 &a, const SrMatrix33 &b)
	{
	data.s._11 = a.data.s._11 - b.data.s._11;
	data.s._12 = a.data.s._12 - b.data.s._12;
	data.s._13 = a.data.s._13 - b.data.s._13;

	data.s._21 = a.data.s._21 - b.data.s._21;
	data.s._22 = a.data.s._22 - b.data.s._22;
	data.s._23 = a.data.s._23 - b.data.s._23;

	data.s._31 = a.data.s._31 - b.data.s._31;
	data.s._32 = a.data.s._32 - b.data.s._32;
	data.s._33 = a.data.s._33 - b.data.s._33;
	}


SR_INLINE void SrMatrix33::multiply(float d,  const SrMatrix33 & a)
	{
	data.s._11 = a.data.s._11 * d;
	data.s._12 = a.data.s._12 * d;
	data.s._13 = a.data.s._13 * d;

	data.s._21 = a.data.s._21 * d;
	data.s._22 = a.data.s._22 * d;
	data.s._23 = a.data.s._23 * d;

	data.s._31 = a.data.s._31 * d;
	data.s._32 = a.data.s._32 * d;
	data.s._33 = a.data.s._33 * d;
	}


SR_INLINE void SrMatrix33::multiply(const SrMatrix33& left, const SrMatrix33& right)
	{
	float a,b,c, d,e,f, g,h,i;
	//note: temps needed so that x.multiply(x,y) works OK.
	a =left.data.s._11 * right.data.s._11 +left.data.s._12 * right.data.s._21 +left.data.s._13 * right.data.s._31;
	b =left.data.s._11 * right.data.s._12 +left.data.s._12 * right.data.s._22 +left.data.s._13 * right.data.s._32;
	c =left.data.s._11 * right.data.s._13 +left.data.s._12 * right.data.s._23 +left.data.s._13 * right.data.s._33;

	d =left.data.s._21 * right.data.s._11 +left.data.s._22 * right.data.s._21 +left.data.s._23 * right.data.s._31;
	e =left.data.s._21 * right.data.s._12 +left.data.s._22 * right.data.s._22 +left.data.s._23 * right.data.s._32;
	f =left.data.s._21 * right.data.s._13 +left.data.s._22 * right.data.s._23 +left.data.s._23 * right.data.s._33;

	g =left.data.s._31 * right.data.s._11 +left.data.s._32 * right.data.s._21 +left.data.s._33 * right.data.s._31;
	h =left.data.s._31 * right.data.s._12 +left.data.s._32 * right.data.s._22 +left.data.s._33 * right.data.s._32;
	i =left.data.s._31 * right.data.s._13 +left.data.s._32 * right.data.s._23 +left.data.s._33 * right.data.s._33;


	data.s._11 = a;
	data.s._12 = b;
	data.s._13 = c;

	data.s._21 = d;
	data.s._22 = e;
	data.s._23 = f;

	data.s._31 = g;
	data.s._32 = h;
	data.s._33 = i;
	}


SR_INLINE void SrMatrix33::multiplyTransposeLeft(const SrMatrix33& left, const SrMatrix33& right)
	{
	float a,b,c, d,e,f, g,h,i;
	//note: temps needed so that x.multiply(x,y) works OK.
	a =left.data.s._11 * right.data.s._11 +left.data.s._21 * right.data.s._21 +left.data.s._31 * right.data.s._31;
	b =left.data.s._11 * right.data.s._12 +left.data.s._21 * right.data.s._22 +left.data.s._31 * right.data.s._32;
	c =left.data.s._11 * right.data.s._13 +left.data.s._21 * right.data.s._23 +left.data.s._31 * right.data.s._33;

	d =left.data.s._12 * right.data.s._11 +left.data.s._22 * right.data.s._21 +left.data.s._32 * right.data.s._31;
	e =left.data.s._12 * right.data.s._12 +left.data.s._22 * right.data.s._22 +left.data.s._32 * right.data.s._32;
	f =left.data.s._12 * right.data.s._13 +left.data.s._22 * right.data.s._23 +left.data.s._32 * right.data.s._33;

	g =left.data.s._13 * right.data.s._11 +left.data.s._23 * right.data.s._21 +left.data.s._33 * right.data.s._31;
	h =left.data.s._13 * right.data.s._12 +left.data.s._23 * right.data.s._22 +left.data.s._33 * right.data.s._32;
	i =left.data.s._13 * right.data.s._13 +left.data.s._23 * right.data.s._23 +left.data.s._33 * right.data.s._33;

	data.s._11 = a;
	data.s._12 = b;
	data.s._13 = c;

	data.s._21 = d;
	data.s._22 = e;
	data.s._23 = f;

	data.s._31 = g;
	data.s._32 = h;
	data.s._33 = i;
	}


SR_INLINE void SrMatrix33::multiplyTransposeRight(const SrMatrix33& left, const SrMatrix33& right)
	{
	float a,b,c, d,e,f, g,h,i;
	//note: temps needed so that x.multiply(x,y) works OK.
	a =left.data.s._11 * right.data.s._11 +left.data.s._12 * right.data.s._12 +left.data.s._13 * right.data.s._13;
	b =left.data.s._11 * right.data.s._21 +left.data.s._12 * right.data.s._22 +left.data.s._13 * right.data.s._23;
	c =left.data.s._11 * right.data.s._31 +left.data.s._12 * right.data.s._32 +left.data.s._13 * right.data.s._33;

	d =left.data.s._21 * right.data.s._11 +left.data.s._22 * right.data.s._12 +left.data.s._23 * right.data.s._13;
	e =left.data.s._21 * right.data.s._21 +left.data.s._22 * right.data.s._22 +left.data.s._23 * right.data.s._23;
	f =left.data.s._21 * right.data.s._31 +left.data.s._22 * right.data.s._32 +left.data.s._23 * right.data.s._33;

	g =left.data.s._31 * right.data.s._11 +left.data.s._32 * right.data.s._12 +left.data.s._33 * right.data.s._13;
	h =left.data.s._31 * right.data.s._21 +left.data.s._32 * right.data.s._22 +left.data.s._33 * right.data.s._23;
	i =left.data.s._31 * right.data.s._31 +left.data.s._32 * right.data.s._32 +left.data.s._33 * right.data.s._33;

	data.s._11 = a;
	data.s._12 = b;
	data.s._13 = c;

	data.s._21 = d;
	data.s._22 = e;
	data.s._23 = f;

	data.s._31 = g;
	data.s._32 = h;
	data.s._33 = i;
	}


SR_INLINE void SrMatrix33::multiplyTransposeRight(const SrVector3 &left, const SrVector3 &right)
	{
	data.s._11 = left.x * right.x;
	data.s._12 = left.x * right.y;
	data.s._13 = left.x * right.z;

	data.s._21 = left.y * right.x;
	data.s._22 = left.y * right.y;
	data.s._23 = left.y * right.z;

	data.s._31 = left.z * right.x;
	data.s._32 = left.z * right.y;
	data.s._33 = left.z * right.z;
	}

SR_INLINE void SrMatrix33::rotX(float angle)
	{
	float Cos = cosf(angle);
	float Sin = sinf(angle);
	id();
	data.m[1][1] = data.m[2][2] = Cos;
	data.m[1][2] = -Sin;
	data.m[2][1] = Sin;
	}

SR_INLINE void SrMatrix33::rotY(float angle)
	{
	float Cos = cosf(angle);
	float Sin = sinf(angle);
	id();
	data.m[0][0] = data.m[2][2] = Cos;
	data.m[0][2] = Sin;
	data.m[2][0] = -Sin;
	}

SR_INLINE void SrMatrix33::rotZ(float angle)
	{
	float Cos = cosf(angle);
	float Sin = sinf(angle);
	id();
	data.m[0][0] = data.m[1][1] = Cos;
	data.m[0][1] = -Sin;
	data.m[1][0] = Sin;
	}

SR_INLINE SrVector3  SrMatrix33::operator%(const SrVector3 & src) const
	{
	SrVector3 dest;
	this->multiplyByTranspose(src, dest);
	return dest;
	}


SR_INLINE SrVector3  SrMatrix33::operator*(const SrVector3 & src) const
	{
	SrVector3 dest;
	this->multiply(src, dest);
	return dest;
	}


SR_INLINE const SrMatrix33 &SrMatrix33::operator +=(const SrMatrix33 &d)
	{
	data.s._11 += d.data.s._11;
	data.s._12 += d.data.s._12;
	data.s._13 += d.data.s._13;

	data.s._21 += d.data.s._21;
	data.s._22 += d.data.s._22;
	data.s._23 += d.data.s._23;

	data.s._31 += d.data.s._31;
	data.s._32 += d.data.s._32;
	data.s._33 += d.data.s._33;
	return *this;
	}


SR_INLINE const SrMatrix33 &SrMatrix33::operator -=(const SrMatrix33 &d)
	{
	data.s._11 -= d.data.s._11;
	data.s._12 -= d.data.s._12;
	data.s._13 -= d.data.s._13;

	data.s._21 -= d.data.s._21;
	data.s._22 -= d.data.s._22;
	data.s._23 -= d.data.s._23;

	data.s._31 -= d.data.s._31;
	data.s._32 -= d.data.s._32;
	data.s._33 -= d.data.s._33;
	return *this;
	}


SR_INLINE const SrMatrix33 &SrMatrix33::operator *=(float f)
	{
	data.s._11 *= f;
	data.s._12 *= f;
	data.s._13 *= f;

	data.s._21 *= f;
	data.s._22 *= f;
	data.s._23 *= f;

	data.s._31 *= f;
	data.s._32 *= f;
	data.s._33 *= f;
	return *this;
	}


SR_INLINE const SrMatrix33 &SrMatrix33::operator /=(float x)
	{
	float f = float(1.0) / x;
	data.s._11 *= f;
	data.s._12 *= f;
	data.s._13 *= f;

	data.s._21 *= f;
	data.s._22 *= f;
	data.s._23 *= f;

	data.s._31 *= f;
	data.s._32 *= f;
	data.s._33 *= f;
	return *this;
	}


SR_INLINE float SrMatrix33::determinant() const
	{
	return  data.s._11*data.s._22*data.s._33 + data.s._12*data.s._23*data.s._31 + data.s._13*data.s._21*data.s._32 
		  - data.s._13*data.s._22*data.s._31 - data.s._12*data.s._21*data.s._33 - data.s._11*data.s._23*data.s._32;
	}


SR_INLINE bool SrMatrix33::getInverse(SrMatrix33& dest) const
	{
	float b00,b01,b02,b10,b11,b12,b20,b21,b22;

	b00 = data.s._22*data.s._33-data.s._23*data.s._32;	b01 = data.s._13*data.s._32-data.s._12*data.s._33;	b02 = data.s._12*data.s._23-data.s._13*data.s._22;
	b10 = data.s._23*data.s._31-data.s._21*data.s._33;	b11 = data.s._11*data.s._33-data.s._13*data.s._31;	b12 = data.s._13*data.s._21-data.s._11*data.s._23;
	b20 = data.s._21*data.s._32-data.s._22*data.s._31;	b21 = data.s._12*data.s._31-data.s._11*data.s._32;	b22 = data.s._11*data.s._22-data.s._12*data.s._21;
	


	/*
	compute determinant: 
	float d =   a00*a11*a22 + a01*a12*a20 + a02*a10*a21	- a02*a11*a20 - a01*a10*a22 - a00*a12*a21;
				0				1			2			3				4			5

	this is a subset of the multiplies done above:

	float d = b00*a00				+		b01*a10						 + b02 * a20;
	float d = (a11*a22-a12*a21)*a00 +		(a02*a21-a01*a22)*a10		 + (a01*a12-a02*a11) * a20;

	float d = a11*a22*a00-a12*a21*a00 +		a02*a21*a10-a01*a22*a10		 + a01*a12*a20-a02*a11*a20;
			0			5					2			4					1			3
	*/

	float d = b00*data.s._11		+		b01*data.s._21				 + b02 * data.s._31;
	
	if (d == float(0.0))		//singular?
		{
		dest.id();
		return false;
		}
	
	d = float(1.0)/d;

	//only do assignment at the end, in case dest == this:


	dest.data.s._11 = b00*d; dest.data.s._12 = b01*d; dest.data.s._13 = b02*d;
	dest.data.s._21 = b10*d; dest.data.s._22 = b11*d; dest.data.s._23 = b12*d;
	dest.data.s._31 = b20*d; dest.data.s._32 = b21*d; dest.data.s._33 = b22*d;

	return true;
	}


SR_INLINE SrMatrix33&	SrMatrix33::operator*= (const SrMatrix33& mat)
	{
	this->multiply(*this, mat);
	return *this;
	}


SR_INLINE SrMatrix33	SrMatrix33::operator-  (const SrMatrix33& mat)	const
	{
	SrMatrix33 temp;
	temp.subtract(*this, mat);
	return temp;
	}


SR_INLINE SrMatrix33	SrMatrix33::operator+  (const SrMatrix33& mat)	const
	{
	SrMatrix33 temp;
	temp.add(*this, mat);
	return temp;
	}


SR_INLINE SrMatrix33	SrMatrix33::operator*  (const SrMatrix33& mat)	const
	{
	SrMatrix33 temp;
	temp.multiply(*this, mat);
	return temp;
	}


SR_INLINE SrMatrix33	SrMatrix33::operator*  (float s)			const
	{
	SrMatrix33 temp;
	temp.multiply(s, *this);
	return temp;
	}

SR_INLINE SrQuaternion::SrQuaternion(const class SrMatrix33 &m)
{
	m.toQuat(*this);
}


/** @} */
#endif

/************************************************************************	
\file 	SrMatrix34.h	
\link	www.twinklingstar.cn
\author Twinkling Star
\date	2013/11/22
****************************************************************************/
#ifndef SR_FOUNDATION_SRMATRIX34T_H_
#define SR_FOUNDATION_SRMATRIX34T_H_
/** \addtogroup foundation
  @{
*/

#include "SrMatrix33.h"

/**
\brief Combination of a 3x3 rotation matrix and a translation vector.

homogenous transform class composed of a matrix and a vector.
*/

class SrMatrix34
	{
	public:
	/**
	\brief [ M t ]
	*/
	SrMatrix33 M;
	SrVector3 t;

	/**
	\brief by default M is inited and t isn't.  Use this ctor to either init or not init in full.
	*/
	SR_INLINE explicit SrMatrix34(bool init = true);

	SR_INLINE SrMatrix34(const SrMatrix33& rot, const SrVector3& trans) : M(rot), t(trans)
		{
		}

	SR_INLINE void zero();

	SR_INLINE void id();

	/**
	\brief returns true for identity matrix
	*/
	SR_INLINE bool isIdentity() const;

	/**
	\brief returns true if all elems are finite (not NAN or INF, etc.)
	*/
	SR_INLINE bool isFinite() const;

	/**
	\brief assigns inverse to dest. 
	
	Returns false if singular (i.e. if no inverse exists), setting dest to identity.  dest may equal this.
	*/
	SR_INLINE bool getInverse(SrMatrix34& dest) const;

	/**
	\brief same as #getInverse(), but assumes that M is orthonormal
	*/
	SR_INLINE bool getInverseRT(SrMatrix34& dest) const;

	/**
	\brief dst = this * src
	*/
	SR_INLINE void multiply(const SrVector3 &src, SrVector3 &dst) const;

	/**
	\brief operator wrapper for multiply
	*/
	SR_INLINE SrVector3 operator*  (const SrVector3 & src) const { SrVector3 dest; multiply(src, dest); return dest; }
	/**
	\brief dst = inverse(this) * src	-- assumes M is rotation matrix!!!
	*/
	SR_INLINE void multiplyByInverseRT(const SrVector3 &src, SrVector3 &dst) const;

	/**
	\brief operator wrapper for multiplyByInverseRT
	*/
	SR_INLINE SrVector3 operator%  (const SrVector3 & src) const { SrVector3 dest; multiplyByInverseRT(src, dest); return dest; }

	/**
	\brief this = left * right	
	*/
	SR_INLINE void multiply(const SrMatrix34& left, const SrMatrix34& right);

	/**
	\brief this = inverse(left) * right	-- assumes M is rotation matrix!!!
	*/
	SR_INLINE void multiplyInverseRTLeft(const SrMatrix34& left, const SrMatrix34& right);

	/**
	\brief this = left * inverse(right)	-- assumes M is rotation matrix!!!
	*/
	SR_INLINE void multiplyInverseRTRight(const SrMatrix34& left, const SrMatrix34& right);

	/**
	\brief operator wrapper for multiply
	*/
	SR_INLINE SrMatrix34 operator*  (const SrMatrix34 & right) const { SrMatrix34 dest(false); dest.multiply(*this, right); return dest; }

	/**
	\brief convert from a matrix format appropriate for rendering
	*/
	SR_INLINE void setColumnMajor44(const SrF32 *);
	/**
	\brief convert from a matrix format appropriate for rendering
	*/
	SR_INLINE void setColumnMajor44(const SrF32 d[4][4]);
	/**
	\brief convert to a matrix format appropriate for rendering
	*/
	SR_INLINE void getColumnMajor44(SrF32 *) const;
	/**
	\brief convert to a matrix format appropriate for rendering
	*/
	SR_INLINE void getColumnMajor44(SrF32 d[4][4]) const;
	/**
	\brief set the matrix given a row major matrix.
	*/
	SR_INLINE void setRowMajor44(const SrF32 *);
	/**
	\brief set the matrix given a row major matrix.
	*/
	SR_INLINE void setRowMajor44(const SrF32 d[4][4]);
	/**
	\brief retrieve the matrix in a row major format.
	*/
	SR_INLINE void getRowMajor44(SrF32 *) const;
	/**
	\brief retrieve the matrix in a row major format.
	*/
	SR_INLINE void getRowMajor44(SrF32 d[4][4]) const;
	};


SR_INLINE SrMatrix34::SrMatrix34(bool init)
	{
	if (init)
	{
		t.zero();
		M.id();
	}
	}


SR_INLINE void SrMatrix34::zero()
	{
	M.zero();
	t.zero();
	}


SR_INLINE void SrMatrix34::id()
	{
	M.id();
	t.zero();
	}


SR_INLINE bool SrMatrix34::isIdentity() const
	{
	if(!M.isIdentity())	return false;
	if(!t.isZero())		return false;
	return true;
	}


SR_INLINE bool SrMatrix34::isFinite() const
	{
	if(!M.isFinite())	return false;
	if(!t.isFinite())	return false;
	return true;
	}


SR_INLINE bool SrMatrix34::getInverse(SrMatrix34& dest) const
	{
	// inv(this) = [ inv(M) , inv(M) * -t ]
	bool status = M.getInverse(dest.M);
	dest.M.multiply(t * -1.0f, dest.t); 
	return status;
	}


SR_INLINE bool SrMatrix34::getInverseRT(SrMatrix34& dest) const
	{
	// inv(this) = [ M' , M' * -t ]
	dest.M.setTransposed(M);
	dest.M.multiply(t * -1.0f, dest.t); 
	return true;
	}



SR_INLINE void SrMatrix34::multiply(const SrVector3 &src, SrVector3 &dst) const
	{
	dst = M * src + t;
	}


SR_INLINE void SrMatrix34::multiplyByInverseRT(const SrVector3 &src, SrVector3 &dst) const
	{
	//dst = M' * src - M' * t = M' * (src - t)
	M.multiplyByTranspose(src - t, dst);
	}


SR_INLINE void SrMatrix34::multiply(const SrMatrix34& left, const SrMatrix34& right)
	{
	//[aR at] * [bR bt] = [aR * bR		aR * bt + at]  NOTE: order of operations important so it works when this ?= left ?= right.
	t = left.M * right.t + left.t;
	M.multiply(left.M, right.M);
	}


SR_INLINE void SrMatrix34::multiplyInverseRTLeft(const SrMatrix34& left, const SrMatrix34& right)
	{
	//[aR' -aR'*at] * [bR bt] = [aR' * bR		aR' * bt  - aR'*at]	//aR' ( bt  - at )	NOTE: order of operations important so it works when this ?= left ?= right.
	t = left.M % (right.t - left.t);
	M.multiplyTransposeLeft(left.M, right.M);
	}


SR_INLINE void SrMatrix34::multiplyInverseRTRight(const SrMatrix34& left, const SrMatrix34& right)
	{
	//[aR at] * [bR' -bR'*bt] = [aR * bR'		-aR * bR' * bt + at]	NOTE: order of operations important so it works when this ?= left ?= right.
	M.multiplyTransposeRight(left.M, right.M);
	t = left.t - M * right.t;
	}

SR_INLINE void SrMatrix34::setColumnMajor44(const SrF32 * d) 
	{
	M.setColumnMajorStride4(d);
    t.x = d[12];
	t.y = d[13];
	t.z = d[14];
	}

SR_INLINE void SrMatrix34::setColumnMajor44(const SrF32 d[4][4]) 
	{
	M.setColumnMajorStride4(d);
    t.x = d[3][0];
	t.y = d[3][1];
	t.z = d[3][2];
	}

SR_INLINE void SrMatrix34::getColumnMajor44(SrF32 * d) const
	{
	M.getColumnMajorStride4(d);
    d[12] = t.x;
	d[13] = t.y;
	d[14] = t.z;
	d[3] = d[7] = d[11] = 0.0f;
	d[15] = 1.0f;
	}

SR_INLINE void SrMatrix34::getColumnMajor44(SrF32 d[4][4]) const
	{
	M.getColumnMajorStride4(d);
    d[3][0] = t.x;
	d[3][1] = t.y;
	d[3][2] = t.z;
	d[0][3] = d[1][3] = d[2][3] = 0.0f;
	d[3][3] = 1.0f;
	}

SR_INLINE void SrMatrix34::setRowMajor44(const SrF32 * d) 
	{
	M.setRowMajorStride4(d);
    t.x = d[3];
	t.y = d[7];
	t.z = d[11];
	}

SR_INLINE void SrMatrix34::setRowMajor44(const SrF32 d[4][4])
	{
	M.setRowMajorStride4(d);
    t.x = d[0][3];
	t.y = d[1][3];
	t.z = d[2][3];
	}

SR_INLINE void SrMatrix34::getRowMajor44(SrF32 * d) const
	{
	M.getRowMajorStride4(d);
    d[3] = t.x;
	d[7] = t.y;
	d[11] = t.z;
	d[12] = d[13] = d[14] = 0.0f;
	d[15] = 1.0f;
	}

SR_INLINE void SrMatrix34::getRowMajor44(SrF32 d[4][4]) const
	{
	M.getRowMajorStride4(d);
    d[0][3] = t.x;
	d[1][3] = t.y;
	d[2][3] = t.z;
	d[3][0] = d[3][1] = d[3][2] = 0.0f;
	d[3][3] = 1.0f;
	}
/** @} */
#endif

SrVector2和SrVector3类的实现源码,以及相关的SrMath类和相关的类型定义文件SrSimpleTypes.h。

/************************************************************************		
\file 	SrVector2.h
\link	www.twinklingstar.cn
\author Twinkling Star
\date	2013/11/22
****************************************************************************/
#ifndef	SR_FOUNDATION_SRVECTOR2_H_
#define SR_FOUNDATION_SRVECTOR2_H_
/** \addtogroup foundation
  @{
*/

#include <assert.h>
#include "SrSimpleTypes.h"
#include "SrMath.h"



#ifndef SR_ASSERT
#ifdef _DEBUG
#define SR_ASSERT(x) assert(x)
#else
#define SR_ASSERT(x) {}
#endif
#endif


/**
\brief 2 Element vector class.

This is a vector class with public data members.
This is not nice but it has become such a standard that hiding the xy data members
makes it difficult to reuse external code that assumes that these are public in the library.
The vector class can be made to use float or double precision by appropriately defining float.
This has been chosen as a cleaner alternative to a template class.
*/
class SrVector2
	{
	public:
	//!Constructors

	/**
	\brief default constructor leaves data uninitialized.
	*/
	SR_INLINE SrVector2();

	/**
	\brief Assigns scalar parameter to all elements.
	
	Useful to initialize to zero or one.

	\param[in] a Value to assign to elements.
	*/
	SR_INLINE explicit SrVector2(SrReal a);

	/**
	\brief Initializes from 2 scalar parameters.

	\param[in] nx Value to initialize X component.
	\param[in] ny Value to initialize Y component.
	*/
	SR_INLINE SrVector2(SrReal nx, SrReal ny);
	
	/**
	\brief Initializes from an array of scalar parameters.

	\param[in] v Value to initialize with.
	*/
	SR_INLINE SrVector2(const SrReal v[]);

	/**
	\brief Copy constructor.
	*/
	SR_INLINE SrVector2(const SrVector2& v);

	/**
	\brief Assignment operator.
	*/
	SR_INLINE const SrVector2& operator=(const SrVector2&);

	/**
	\brief Access the data as an array.

	\return Array of 2 floats.
	*/
	SR_INLINE const SrReal *get() const;
	/**
	\brief writes out the 2 values to dest.

	\param[out] dest Array to write elements to.
	*/
	SR_INLINE void get(SrF32 * dest) const;

	/**
	\brief writes out the 2 values to dest.
	*/
	SR_INLINE void get(SrF64 * dest) const;

	/**
	\brief Access the data as an array.

	\return Array of 2 floats.
	*/
	SR_INLINE SrReal* get();

	SR_INLINE SrReal& operator[](int index);
	SR_INLINE SrReal  operator[](int index) const;

	//Operators
	/**
	\brief true if all the members are smaller.
	*/
	SR_INLINE bool operator< (const SrVector2&) const;
	/**
	\brief returns true if the two vectors are exactly equal.

	use equal() to test with a tolerance.
	*/
	SR_INLINE bool operator==(const SrVector2&) const;
	/**
	\brief returns true if the two vectors are exactly unequal.

	use !equal() to test with a tolerance.
	*/
	SR_INLINE bool operator!=(const SrVector2&) const;

//Methods
	
	/**
	\brief reads 2 consecutive values from the ptr passed
	*/
	SR_INLINE void  set(const SrF32 *);

	/**
	\brief reads 2 consecutive values from the ptr passed
	*/
	SR_INLINE void  set(const SrF64 *);
	SR_INLINE void  set(const SrVector2 &);

//legacy methods:
	SR_INLINE void setx(const SrReal & d);
	SR_INLINE void sety(const SrReal & d);

	/**
	\brief this = -a
	*/
	SR_INLINE void  setNegative(const SrVector2 &a);

	/**
	\brief this = -this
	*/
	SR_INLINE void  setNegative();

	/**
	\brief reads 2 consecutive values from the ptr passed
	*/
	SR_INLINE void  set(SrReal, SrReal);
	SR_INLINE void  set(SrReal);

	SR_INLINE void  zero();
	
	/**
	\brief tests for exact zero vector
	*/
	SR_INLINE bool isZero()	const
		{
		if((x != 0.0f) || (y != 0.0f))	return false;
		return true;
		}

	SR_INLINE void  setPlusInfinity();
	SR_INLINE void  setMinusInfinity();

	/**
	\brief this = element wise min(this,other)
	*/
	SR_INLINE void min(const SrVector2 &);
	/**
	\brief this = element wise max(this,other)
	*/
	SR_INLINE void max(const SrVector2 &);

	/**
	\brief this = a + b
	*/
	SR_INLINE void  add(const SrVector2 & a, const SrVector2 & b);
	/**
	\brief this = a - b
	*/
	SR_INLINE void  subtract(const SrVector2 &a, const SrVector2 &b);
	/**
	\brief this = s * a;
	*/
	SR_INLINE void  multiply(SrReal s,  const SrVector2 & a);

	/**
	\brief this[i] = a[i] * b[i], for all i.
	*/
	SR_INLINE void  arrayMultiply(const SrVector2 &a, const SrVector2 &b);


	/**
	\brief this = s * a + b;
	*/
	SR_INLINE void  multiplyAdd(SrReal s, const SrVector2 & a, const SrVector2 & b);

	/**
	\brief normalizes the vector
	*/
	SR_INLINE SrReal normalize();

	/**
	\brief sets the vector's magnitude
	*/
	SR_INLINE void	setMagnitude(SrReal);

	/**
	\brief snaps to closest axis
	*/
	SR_INLINE SrU32	closestAxis()	const;

	/**
	\brief returns true if all 2 elems of the vector are finite (not NAN or INF, etc.)
	*/
	SR_INLINE bool isFinite() const;

	/**
	\brief returns the scalar product of this and other.
	*/
	SR_INLINE SrReal dot(const SrVector2 &other) const;
	/**
	\brief returns the scalar cross product of this and other.
	*/
	SR_INLINE SrReal cross(const SrVector2 &other) const;

	/**
	\brief compares orientations (more readable, user-friendly function)
	*/
	SR_INLINE bool sameDirection(const SrVector2 &) const;

	/**
	\brief returns the magnitude
	*/
	SR_INLINE SrReal magnitude() const;

	/**
	\brief returns the squared magnitude
	
	Avoids calling sqrt()!
	*/
	SR_INLINE SrReal magnitudeSquared() const;

	/**
	\brief returns (this - other).magnitude();
	*/
	SR_INLINE SrReal distance(const SrVector2 &) const;

	/**
	\brief returns (this - other).magnitudeSquared();
	*/
	SR_INLINE SrReal distanceSquared(const SrVector2 &v) const;

	/**
	\brief Stuff magic values in the point, marking it as explicitly not used.
	*/
	SR_INLINE void setNotUsed();

	/**
	\brief Checks the point is marked as not used
	*/
	SR_INLINE bool isNotUsed() const;

	/**
	\brief returns true if this and arg's elems are within epsilon of each other.
	*/
	SR_INLINE bool equals(const SrVector2 &, SrReal epsilon) const;

	/**
	\brief negation
	*/
	SR_INLINE SrVector2 operator -() const;
	/**
	\brief vector addition
	*/
	SR_INLINE SrVector2 operator +(const SrVector2 & v) const;
	/**
	\brief vector difference
	*/
	SR_INLINE SrVector2 operator -(const SrVector2 & v) const;
	/**
	\brief scalar post-multiplication
	*/
	SR_INLINE SrVector2 operator *(SrReal f) const;
	/**
	\brief scalar division
	*/
	SR_INLINE SrVector2 operator /(SrReal f) const;
	/**
	\brief vector addition
	*/
	SR_INLINE SrVector2&operator +=(const SrVector2& v);
	/**
	\brief vector difference
	*/
	SR_INLINE SrVector2&operator -=(const SrVector2& v);
	/**
	\brief scalar multiplication
	*/
	SR_INLINE SrVector2&operator *=(SrReal f);
	/**
	\brief scalar division
	*/
	SR_INLINE SrVector2&operator /=(SrReal f);
	/**
	\brief dot product
	*/
	SR_INLINE SrReal      operator|(const SrVector2& v) const;

	SrReal x,y;
	};


/** \endcond */

//implementations:
SrVector2::SrVector2(SrReal v) : x(v), y(v)
	{
	}

SR_INLINE SrVector2::SrVector2(SrReal _x, SrReal _y) : x(_x), y(_y)
	{
	}


SR_INLINE SrVector2::SrVector2(const SrReal v[]) : x(v[0]), y(v[1])
	{
	}


SR_INLINE SrVector2::SrVector2(const SrVector2 &v) : x(v.x), y(v.y)
	{
	}


SR_INLINE SrVector2::SrVector2()
	{
	//default constructor leaves data uninitialized.
	}


SR_INLINE const SrVector2& SrVector2::operator=(const SrVector2& v)
	{
	x = v.x;
	y = v.y;
	return *this;
	}


// Access the data as an array.

SR_INLINE const SrReal* SrVector2::get() const
	{
	return &x;
	}


SR_INLINE SrReal* SrVector2::get()
	{
	return &x;
	}

 
SR_INLINE void  SrVector2::get(SrF32 * v) const
	{
	v[0] = (SrF32)x;
	v[1] = (SrF32)y;
	}

 
SR_INLINE void  SrVector2::get(SrF64 * v) const
	{
	v[0] = (SrF64)x;
	v[1] = (SrF64)y;
	}


SR_INLINE SrReal& SrVector2::operator[](int index)
	{
	SR_ASSERT(index>=0 && index<=1);
	return (&x)[index];
	}


SR_INLINE SrReal  SrVector2::operator[](int index) const
	{
	SR_ASSERT(index>=0 && index<=1);
	return (&x)[index];
	}

 
SR_INLINE void SrVector2::setx(const SrReal & d) 
	{ 
	x = d; 
	}

 
SR_INLINE void SrVector2::sety(const SrReal & d) 
	{ 
	y = d; 
	}

//Operators
 
SR_INLINE bool SrVector2::operator < (const SrVector2&v) const
	{
		if( x < v.x)	return true;
		if( x > v.x)	return false;
		if( y < v.y)	return true;
		return false;
	}

 
SR_INLINE bool SrVector2::operator==(const SrVector2& v) const
	{
	return ((x == v.x)&&(y == v.y));
	}

 
SR_INLINE bool SrVector2::operator!=(const SrVector2& v) const
	{
	return ((x != v.x)||(y != v.y));
	}

//Methods
 
SR_INLINE void  SrVector2::set(const SrVector2 & v)
	{
	x = v.x;
	y = v.y;
	}

 
SR_INLINE void  SrVector2::setNegative(const SrVector2 & v)
	{
	x = -v.x;
	y = -v.y;
	}

 
SR_INLINE void  SrVector2::setNegative()
	{
	x = -x;
	y = -y;
	}


 
SR_INLINE void  SrVector2::set(const SrF32 * v)
	{
	x = (SrReal)v[0];
	y = (SrReal)v[1];
	}

 
SR_INLINE void  SrVector2::set(const SrF64 * v)
	{
	x = (SrReal)v[0];
	y = (SrReal)v[1];
	}


 
SR_INLINE void  SrVector2::set(SrReal _x, SrReal _y)
	{
	this->x = _x;
	this->y = _y;
	}

 
SR_INLINE void SrVector2::set(SrReal v)
	{
	x = v;
	y = v;
	}

 
SR_INLINE void  SrVector2::zero()
	{
	x = y = 0.0;
	}

 
SR_INLINE void  SrVector2::setPlusInfinity()
	{
	x = y = SR_MAX_F32; //TODO: this may be double too, but here we can't tell!
	}

 
SR_INLINE void  SrVector2::setMinusInfinity()
	{
	x = y = SR_MIN_F32; //TODO: this may be double too, but here we can't tell!
	}

 
SR_INLINE void SrVector2::max(const SrVector2 & v)
	{
		x = x > v.x?x:v.x;
		y = y > v.y?y:y;
	}

 
SR_INLINE void SrVector2::min(const SrVector2 & v)
	{
		x = x > v.x?v.x:x;
		y = y > v.y?v.y:y;
	}




SR_INLINE void  SrVector2::add(const SrVector2 & a, const SrVector2 & b)
	{
	x = a.x + b.x;
	y = a.y + b.y;
	}


SR_INLINE void  SrVector2::subtract(const SrVector2 &a, const SrVector2 &b)
	{
	x = a.x - b.x;
	y = a.y - b.y;
	}


SR_INLINE void  SrVector2::arrayMultiply(const SrVector2 &a, const SrVector2 &b)
	{
	x = a.x * b.x;
	y = a.y * b.y;
	}


SR_INLINE void  SrVector2::multiply(SrReal s,  const SrVector2 & a)
	{
	x = a.x * s;
	y = a.y * s;
	}


SR_INLINE void  SrVector2::multiplyAdd(SrReal s, const SrVector2 & a, const SrVector2 & b)
	{
	x = s * a.x + b.x;
	y = s * a.y + b.y;
	}

 
SR_INLINE SrReal SrVector2::normalize()
	{
	SrReal m = magnitude();
	if (m)
		{
		const SrReal il =  SrReal(1.0) / m;
		x *= il;
		y *= il;
		}
	return m;
	}

 
SR_INLINE void SrVector2::setMagnitude(SrReal length)
	{
	SrReal m = magnitude();
	if(m)
		{
		SrReal newLength = length / m;
		x *= newLength;
		y *= newLength;
		}
	}
SR_INLINE bool SrVector2::isFinite() const
	{
		return SrMath::isFinite(x) && SrMath::isFinite(y);
	}
SR_INLINE SrReal SrVector2::dot(const SrVector2 &v) const
	{
		return x * v.x + y * v.y;
	}
SR_INLINE SrReal SrVector2::cross(const SrVector2 &other) const
	{
		return x*other.y-y*other.x;
	}
 
SR_INLINE bool SrVector2::sameDirection(const SrVector2 &v) const
	{
	return x*v.x + y*v.y >= 0.0f;
	}

 
SR_INLINE SrReal SrVector2::magnitude() const
	{
	return SrMath::sqrt(x * x + y * y);
	}

 
SR_INLINE SrReal SrVector2::magnitudeSquared() const
	{
	return x * x + y * y;
	}

 
SR_INLINE SrReal SrVector2::distance(const SrVector2 & v) const
	{
	SrReal dx = x - v.x;
	SrReal dy = y - v.y;
	return SrMath::sqrt(dx * dx + dy * dy);
	}

 
SR_INLINE SrReal SrVector2::distanceSquared(const SrVector2 &v) const
	{
	SrReal dx = x - v.x;
	SrReal dy = y - v.y;
	return dx * dx + dy * dy;
	}


 
SR_INLINE bool SrVector2::equals(const SrVector2 & v, SrReal epsilon) const
	{
	return 
		SrMath::equals(x, v.x, epsilon) &&
		SrMath::equals(y, v.y, epsilon);
	}


 
SR_INLINE SrVector2 SrVector2::operator -() const
	{
	return SrVector2(-x, -y);
	}

 
SR_INLINE SrVector2 SrVector2::operator +(const SrVector2 & v) const
	{
	return SrVector2(x + v.x, y + v.y);	// RVO version
	}

 
SR_INLINE SrVector2 SrVector2::operator -(const SrVector2 & v) const
	{
	return SrVector2(x - v.x, y - v.y);	// RVO version
	}



SR_INLINE SrVector2 SrVector2::operator *(SrReal f) const
	{
	return SrVector2(x * f, y * f);	// RVO version
	}


SR_INLINE SrVector2 SrVector2::operator /(SrReal f) const
	{
		f = SrReal(1.0) / f; return SrVector2(x * f, y * f);
	}


SR_INLINE SrVector2& SrVector2::operator +=(const SrVector2& v)
	{
	x += v.x;
	y += v.y;
	return *this;
	}


SR_INLINE SrVector2& SrVector2::operator -=(const SrVector2& v)
	{
	x -= v.x;
	y -= v.y;
	return *this;
	}


SR_INLINE SrVector2& SrVector2::operator *=(SrReal f)
	{
	x *= f;
	y *= f;
	
	return *this;
	}


SR_INLINE SrVector2& SrVector2::operator /=(SrReal f)
	{
	f = 1.0f/f;
	x *= f;
	y *= f;
	
	return *this;
	}


SR_INLINE SrReal SrVector2::operator|(const SrVector2& v) const
	{
	return x * v.x + y * v.y;
	}

/**
scalar pre-multiplication
*/

SR_INLINE SrVector2 operator *(SrReal f, const SrVector2& v)
	{
	return SrVector2(f * v.x, f * v.y);
	}
/** @} */
#endif
/************************************************************************	
\file 	SrVector3.h	
\link	www.twinklingstar.cn
\author Twinkling Star
\date	2013/11/22
****************************************************************************/
#ifndef	SR_FOUNDATION_SRVECTOR3_H_
#define SR_FOUNDATION_SRVECTOR3_H_
#include <assert.h>
#include "SrSimpleTypes.h"
#include "SrMath.h"
/** \addtogroup foundation
  @{
*/

#ifndef SR_ASSERT
#ifdef _DEBUG
#define SR_ASSERT(x) assert(x)
#else
#define SR_ASSERT(x) {}
#endif
#endif


/**
\brief 3 Element vector class.

This is a vector class with public data members.
This is not nice but it has become such a standard that hiding the xyz data members
makes it difficult to reuse external code that assumes that these are public in the library.
The vector class can be made to use float or double precision by appropriately defining float.
This has been chosen as a cleaner alternative to a template class.
*/
class SrVector3
	{
	public:
	//!Constructors

	/**
	\brief default constructor leaves data uninitialized.
	*/
	SR_INLINE SrVector3();

	/**
	\brief Assigns scalar parameter to all elements.
	
	Useful to initialize to zero or one.

	\param[in] a Value to assign to elements.
	*/
	SR_INLINE explicit SrVector3(SrReal a);

	/**
	\brief Initializes from 3 scalar parameters.

	\param[in] nx Value to initialize X component.
	\param[in] ny Value to initialize Y component.
	\param[in] nz Value to initialize Z component.
	*/
	SR_INLINE SrVector3(SrReal nx, SrReal ny, SrReal nz);
	
	/**
	\brief Initializes from an array of scalar parameters.

	\param[in] v Value to initialize with.
	*/
	SR_INLINE SrVector3(const SrReal v[]);

	/**
	\brief Copy constructor.
	*/
	SR_INLINE SrVector3(const SrVector3& v);

	/**
	\brief Assignment operator.
	*/
	SR_INLINE const SrVector3& operator=(const SrVector3&);

	/**
	\brief Access the data as an array.

	\return Array of 3 SrReals.
	*/
	SR_INLINE const SrReal *get() const;
	/**
	\brief writes out the 3 values to dest.

	\param[out] dest Array to write elements to.
	*/
	SR_INLINE void get(SrF32 * dest) const;

	/**
	\brief writes out the 3 values to dest.
	*/
	SR_INLINE void get(SrF64 * dest) const;

	/**
	\brief Access the data as an array.

	\return Array of 3 SrReals.
	*/
	SR_INLINE SrReal* get();

	SR_INLINE SrReal& operator[](int index);
	SR_INLINE SrReal  operator[](int index) const;

	//Operators
	/**
	\brief true if all the members are smaller.
	*/
	SR_INLINE bool operator< (const SrVector3&) const;
	/**
	\brief returns true if the two vectors are exactly equal.

	use equal() to test with a tolerance.
	*/
	SR_INLINE bool operator==(const SrVector3&) const;
	/**
	\brief returns true if the two vectors are exactly unequal.

	use !equal() to test with a tolerance.
	*/
	SR_INLINE bool operator!=(const SrVector3&) const;

//Methods
	
	/**
	\brief reads 3 consecutive values from the ptr passed
	*/
	SR_INLINE void  set(const SrF32 *);

	/**
	\brief reads 3 consecutive values from the ptr passed
	*/
	SR_INLINE void  set(const SrF64 *);
	SR_INLINE void  set(const SrVector3 &);

//legacy methods:
	SR_INLINE void setx(const SrReal & d);
	SR_INLINE void sety(const SrReal & d);
	SR_INLINE void setz(const SrReal & d);

	/**
	\brief this = -a
	*/
	SR_INLINE void  setNegative(const SrVector3 &a);

	/**
	\brief this = -this
	*/
	SR_INLINE void  setNegative();

	/**
	\brief reads 3 consecutive values from the ptr passed
	*/
	SR_INLINE void  set(SrReal, SrReal, SrReal);
	SR_INLINE void  set(SrReal);

	SR_INLINE void  zero();
	
	/**
	\brief tests for exact zero vector
	*/
	SR_INLINE bool isZero()	const
		{
		if((x != 0.0f) || (y != 0.0f) || (z != 0.0f))	return false;
		return true;
		}

	SR_INLINE void  setPlusInfinity();
	SR_INLINE void  setMinusInfinity();

	/**
	\brief this = element wise min(this,other)
	*/
	SR_INLINE void min(const SrVector3 &);
	/**
	\brief this = element wise max(this,other)
	*/
	SR_INLINE void max(const SrVector3 &);

	/**
	\brief this = a + b
	*/
	SR_INLINE void  add(const SrVector3 & a, const SrVector3 & b);
	/**
	\brief this = a - b
	*/
	SR_INLINE void  subtract(const SrVector3 &a, const SrVector3 &b);
	/**
	\brief this = s * a;
	*/
	SR_INLINE void  multiply(SrReal s,  const SrVector3 & a);

	/**
	\brief this[i] = a[i] * b[i], for all i.
	*/
	SR_INLINE void  arrayMultiply(const SrVector3 &a, const SrVector3 &b);


	/**
	\brief this = s * a + b;
	*/
	SR_INLINE void  multiplyAdd(SrReal s, const SrVector3 & a, const SrVector3 & b);

	/**
	\brief normalizes the vector
	*/
	SR_INLINE SrReal normalize();

	/**
	\brief sets the vector's magnitude
	*/
	SR_INLINE void	setMagnitude(SrReal);

	/**
	\brief snaps to closest axis
	*/
	SR_INLINE SrU32	closestAxis()	const;

	/**
	\brief returns true if all 3 elems of the vector are finite (not NAN or INF, etc.)
	*/
	SR_INLINE bool isFinite() const;

	/**
	\brief returns the scalar product of this and other.
	*/
	SR_INLINE SrReal dot(const SrVector3 &other) const;

	/**
	\brief compares orientations (more readable, user-friendly function)
	*/
	SR_INLINE bool sameDirection(const SrVector3 &) const;

	/**
	\brief returns the magnitude
	*/
	SR_INLINE SrReal magnitude() const;

	/**
	\brief returns the squared magnitude
	
	Avoids calling sqrt()!
	*/
	SR_INLINE SrReal magnitudeSquared() const;

	/**
	\brief returns (this - other).magnitude();
	*/
	SR_INLINE SrReal distance(const SrVector3 &) const;

	/**
	\brief returns (this - other).magnitudeSquared();
	*/
	SR_INLINE SrReal distanceSquared(const SrVector3 &v) const;

	/**
	\brief this = left x right
	*/
	SR_INLINE void cross(const SrVector3 &left, const SrVector3 & right);

	/**
	\brief Stuff magic values in the point, marking it as explicitly not used.
	*/
	SR_INLINE void setNotUsed();

	/**
	\brief Checks the point is marked as not used
	*/
	SR_INLINE bool isNotUsed() const;

	/**
	\brief returns true if this and arg's elems are within epsilon of each other.
	*/
	SR_INLINE bool equals(const SrVector3 &, SrReal epsilon) const;

	/**
	\brief negation
	*/
	SR_INLINE SrVector3 operator -() const;
	/**
	\brief vector addition
	*/
	SR_INLINE SrVector3 operator +(const SrVector3 & v) const;
	/**
	\brief vector difference
	*/
	SR_INLINE SrVector3 operator -(const SrVector3 & v) const;
	/**
	\brief scalar post-multiplication
	*/
	SR_INLINE SrVector3 operator *(SrReal f) const;
	/**
	\brief scalar division
	*/
	SR_INLINE SrVector3 operator /(SrReal f) const;
	/**
	\brief vector addition
	*/
	SR_INLINE SrVector3&operator +=(const SrVector3& v);
	/**
	\brief vector difference
	*/
	SR_INLINE SrVector3&operator -=(const SrVector3& v);
	/**
	\brief scalar multiplication
	*/
	SR_INLINE SrVector3&operator *=(SrReal f);
	/**
	\brief scalar division
	*/
	SR_INLINE SrVector3&operator /=(SrReal f);
	/**
	\brief cross product
	*/
	SR_INLINE SrVector3 cross(const SrVector3& v) const;

	/**
	\brief cross product
	*/
	SR_INLINE SrVector3 operator^(const SrVector3& v) const;
	/**
	\brief dot product
	*/
	SR_INLINE SrReal      operator|(const SrVector3& v) const;

	SrReal x,y,z;
	};


/** \endcond */

//implementations:
SrVector3::SrVector3(SrReal v) : x(v), y(v), z(v)
	{
	}

SR_INLINE SrVector3::SrVector3(SrReal _x, SrReal _y, SrReal _z) : x(_x), y(_y), z(_z)
	{
	}


SR_INLINE SrVector3::SrVector3(const SrReal v[]) : x(v[0]), y(v[1]), z(v[2])
	{
	}


SR_INLINE SrVector3::SrVector3(const SrVector3 &v) : x(v.x), y(v.y), z(v.z)
	{
	}


SR_INLINE SrVector3::SrVector3()
	{
	//default constructor leaves data uninitialized.
	}


SR_INLINE const SrVector3& SrVector3::operator=(const SrVector3& v)
	{
	x = v.x;
	y = v.y;
	z = v.z;
	return *this;
	}


// Access the data as an array.

SR_INLINE const SrReal* SrVector3::get() const
	{
	return &x;
	}


SR_INLINE SrReal* SrVector3::get()
	{
	return &x;
	}

 
SR_INLINE void  SrVector3::get(SrF32 * v) const
	{
	v[0] = (SrF32)x;
	v[1] = (SrF32)y;
	v[2] = (SrF32)z;
	}

 
SR_INLINE void  SrVector3::get(SrF64 * v) const
	{
	v[0] = (SrF64)x;
	v[1] = (SrF64)y;
	v[2] = (SrF64)z;
	}


SR_INLINE SrReal& SrVector3::operator[](int index)
	{
	SR_ASSERT(index>=0 && index<=2);
	return (&x)[index];
	}


SR_INLINE SrReal  SrVector3::operator[](int index) const
	{
	SR_ASSERT(index>=0 && index<=2);
	return (&x)[index];
	}

 
SR_INLINE void SrVector3::setx(const SrReal & d) 
	{ 
	x = d; 
	}

 
SR_INLINE void SrVector3::sety(const SrReal & d) 
	{ 
	y = d; 
	}

 
SR_INLINE void SrVector3::setz(const SrReal & d) 
	{ 
	z = d; 
	}

//Operators
 
SR_INLINE bool SrVector3::operator< (const SrVector3&v) const
	{
		if( x < v.x)	return true;
		if( x > v.x)	return false;
		if( y < v.y)	return true;
		if( y > v.y)	return false;
		if( z < v.z)	return true;
		if( z > v.z)	return false;
		return false;
	}

 
SR_INLINE bool SrVector3::operator==(const SrVector3& v) const
	{
	return ((x == v.x)&&(y == v.y)&&(z == v.z));
	}

 
SR_INLINE bool SrVector3::operator!=(const SrVector3& v) const
	{
	return ((x != v.x)||(y != v.y)||(z != v.z));
	}

//Methods
 
SR_INLINE void  SrVector3::set(const SrVector3 & v)
	{
	x = v.x;
	y = v.y;
	z = v.z;
	}

 
SR_INLINE void  SrVector3::setNegative(const SrVector3 & v)
	{
	x = -v.x;
	y = -v.y;
	z = -v.z;
	}

 
SR_INLINE void  SrVector3::setNegative()
	{
	x = -x;
	y = -y;
	z = -z;
	}


 
SR_INLINE void  SrVector3::set(const SrF32 * v)
	{
	x = (SrReal)v[0];
	y = (SrReal)v[1];
	z = (SrReal)v[2];
	}

 
SR_INLINE void  SrVector3::set(const SrF64 * v)
	{
	x = (SrReal)v[0];
	y = (SrReal)v[1];
	z = (SrReal)v[2];
	}


 
SR_INLINE void  SrVector3::set(SrReal _x, SrReal _y, SrReal _z)
	{
	this->x = _x;
	this->y = _y;
	this->z = _z;
	}

 
SR_INLINE void SrVector3::set(SrReal v)
	{
	x = v;
	y = v;
	z = v;
	}

 
SR_INLINE void  SrVector3::zero()
	{
	x = y = z = 0.0;
	}

 
SR_INLINE void  SrVector3::setPlusInfinity()
	{
	x = y = z = SR_MAX_F32; //TODO: this may be double too, but here we can't tell!
	}

 
SR_INLINE void  SrVector3::setMinusInfinity()
	{
	x = y = z = SR_MIN_F32; //TODO: this may be double too, but here we can't tell!
	}

 
SR_INLINE void SrVector3::max(const SrVector3 & v)
	{
		x = x > v.x?x:v.x;
		y = y > v.y?y:v.y;
		z = z > v.z?z:v.z;
	}

 
SR_INLINE void SrVector3::min(const SrVector3 & v)
	{
		x = x > v.x?v.x:x;
		y = y > v.y?v.y:y;
		z = z > v.z?v.z:z;
	}




SR_INLINE void  SrVector3::add(const SrVector3 & a, const SrVector3 & b)
	{
	x = a.x + b.x;
	y = a.y + b.y;
	z = a.z + b.z;
	}


SR_INLINE void  SrVector3::subtract(const SrVector3 &a, const SrVector3 &b)
	{
	x = a.x - b.x;
	y = a.y - b.y;
	z = a.z - b.z;
	}


SR_INLINE void  SrVector3::arrayMultiply(const SrVector3 &a, const SrVector3 &b)
	{
	x = a.x * b.x;
	y = a.y * b.y;
	z = a.z * b.z;
	}


SR_INLINE void  SrVector3::multiply(SrReal s,  const SrVector3 & a)
	{
	x = a.x * s;
	y = a.y * s;
	z = a.z * s;
	}


SR_INLINE void  SrVector3::multiplyAdd(SrReal s, const SrVector3 & a, const SrVector3 & b)
	{
	x = s * a.x + b.x;
	y = s * a.y + b.y;
	z = s * a.z + b.z;
	}

 
SR_INLINE SrReal SrVector3::normalize()
	{
	SrReal m = magnitude();
	if (m)
		{
		const SrReal il =  SrReal(1.0) / m;
		x *= il;
		y *= il;
		z *= il;
		}
	return m;
	}

 
SR_INLINE void SrVector3::setMagnitude(SrReal length)
	{
	SrReal m = magnitude();
	if(m)
		{
		SrReal newLength = length / m;
		x *= newLength;
		y *= newLength;
		z *= newLength;
		}
	}
SR_INLINE bool SrVector3::isFinite() const
	{
		return SrMath::isFinite(x) && SrMath::isFinite(y) && SrMath::isFinite(z);
	}
SR_INLINE SrReal SrVector3::dot(const SrVector3 &v) const
	{
	return x * v.x + y * v.y + z * v.z;
	}

 
SR_INLINE bool SrVector3::sameDirection(const SrVector3 &v) const
	{
	return x*v.x + y*v.y + z*v.z >= 0.0f;
	}

 
SR_INLINE SrReal SrVector3::magnitude() const
	{
	return SrMath::sqrt(x * x + y * y + z * z);
	}

 
SR_INLINE SrReal SrVector3::magnitudeSquared() const
	{
	return x * x + y * y + z * z;
	}

 
SR_INLINE SrReal SrVector3::distance(const SrVector3 & v) const
	{
	SrReal dx = x - v.x;
	SrReal dy = y - v.y;
	SrReal dz = z - v.z;
	return SrMath::sqrt(dx * dx + dy * dy + dz * dz);
	}

 
SR_INLINE SrReal SrVector3::distanceSquared(const SrVector3 &v) const
	{
	SrReal dx = x - v.x;
	SrReal dy = y - v.y;
	SrReal dz = z - v.z;
	return dx * dx + dy * dy + dz * dz;
	}

 
SR_INLINE void SrVector3::cross(const SrVector3 &left, const SrVector3 & right)	//prefered version, w/o temp object.
	{
	// temps needed in case left or right is this.
	SrReal a = (left.y * right.z) - (left.z * right.y);
	SrReal b = (left.z * right.x) - (left.x * right.z);
	SrReal c = (left.x * right.y) - (left.y * right.x);

	x = a;
	y = b;
	z = c;
	}

 
SR_INLINE bool SrVector3::equals(const SrVector3 & v, SrReal epsilon) const
	{
	return 
		SrMath::equals(x, v.x, epsilon) &&
		SrMath::equals(y, v.y, epsilon) &&
		SrMath::equals(z, v.z, epsilon);
	}


 
SR_INLINE SrVector3 SrVector3::operator -() const
	{
	return SrVector3(-x, -y, -z);
	}

 
SR_INLINE SrVector3 SrVector3::operator +(const SrVector3 & v) const
	{
	return SrVector3(x + v.x, y + v.y, z + v.z);	// RVO version
	}

 
SR_INLINE SrVector3 SrVector3::operator -(const SrVector3 & v) const
	{
	return SrVector3(x - v.x, y - v.y, z - v.z);	// RVO version
	}



SR_INLINE SrVector3 SrVector3::operator *(SrReal f) const
	{
	return SrVector3(x * f, y * f, z * f);	// RVO version
	}


SR_INLINE SrVector3 SrVector3::operator /(SrReal f) const
	{
		f = SrReal(1.0) / f; return SrVector3(x * f, y * f, z * f);
	}


SR_INLINE SrVector3& SrVector3::operator +=(const SrVector3& v)
	{
	x += v.x;
	y += v.y;
	z += v.z;
	return *this;
	}


SR_INLINE SrVector3& SrVector3::operator -=(const SrVector3& v)
	{
	x -= v.x;
	y -= v.y;
	z -= v.z;
	return *this;
	}


SR_INLINE SrVector3& SrVector3::operator *=(SrReal f)
	{
	x *= f;
	y *= f;
	z *= f;
	
	return *this;
	}


SR_INLINE SrVector3& SrVector3::operator /=(SrReal f)
	{
	f = 1.0f/f;
	x *= f;
	y *= f;
	z *= f;
	
	return *this;
	}


SR_INLINE SrVector3 SrVector3::cross(const SrVector3& v) const
	{
	SrVector3 temp;
	temp.cross(*this,v);
	return temp;
	}


SR_INLINE SrVector3 SrVector3::operator^(const SrVector3& v) const
	{
	SrVector3 temp;
	temp.cross(*this,v);
	return temp;
	}


SR_INLINE SrReal SrVector3::operator|(const SrVector3& v) const
	{
	return x * v.x + y * v.y + z * v.z;
	}

/**
scalar pre-multiplication
*/

SR_INLINE SrVector3 operator *(SrReal f, const SrVector3& v)
	{
	return SrVector3(f * v.x, f * v.y, f * v.z);
	}


/** @} */
#endif
/************************************************************************		
\file 	SrSimpleTypes.h
\link	www.twinklingstar.cn
\author Twinkling Star
\date	2013/11/22
****************************************************************************/
#ifndef SR_FOUNDATION_SRSIMPLETYPES_H_
#define SR_FOUNDATION_SRSIMPLETYPES_H_
/** \addtogroup foundation
  @{
*/


typedef signed int			SrI32;
typedef signed short		SrI16;
typedef signed char			SrI8;

typedef unsigned int		SrU32;
typedef unsigned short		SrU16;


typedef float				SrF32;
typedef double				SrF64;


// Type ranges
#define	SR_MAX_I8			0x7f			//max possible sbyte value
#define	SR_MIN_I8			0x80			//min possible sbyte value
#define	SR_MAX_U8			0xff			//max possible ubyte value
#define	SR_MIN_U8			0x00			//min possible ubyte value
#define	SR_MAX_I16			0x7fff			//max possible sword value
#define	SR_MIN_I16			0x8000			//min possible sword value
#define	SR_MAX_U16			0xffff			//max possible uword value
#define	SR_MIN_U16			0x0000			//min possible uword value
#define	SR_MAX_I32			0x7fffffff		//max possible sdword value
#define	SR_MIN_I32			0x80000000		//min possible sdword value
#define	SR_MAX_U32			0xffffffff		//max possible udword value
#define	SR_MIN_U32			0x00000000		//min possible udword value
#define	SR_MAX_F32			3.402823466e+38F			//max possible float value
#define	SR_MIN_F32			(-3.402823466e+38F)		//min possible float value
#define	SR_MAX_F64			1.7976931348623158e+308			//max possible double value
#define	SR_MIN_F64			(-1.7976931348623158e+308)		//min possible double value

#define SR_EPS_F32			FLT_EPSILON		//smallest number not zero
#define SR_EPS_F64			DBL_EPSILON		//smallest number not zero


#define SR_INLINE			inline



/** @} */
#endif
/************************************************************************		
\file 	SrMath.h
\link	www.twinklingstar.cn
\author Twinkling Star
\date	2013/11/22
****************************************************************************/
#ifndef SR_FOUNDATION_SRMAT_H_
#define SR_FOUNDATION_SRMAT_H_

/** \addtogroup foundation
  @{
*/

#include <math.h>
#include <float.h>
#include <stdlib.h>
#include "SrSimpleTypes.h"

#ifdef log2
#undef log2
#endif 
#ifdef min
#undef min
#endif 
#ifdef max
#undef max
#endif 
//constants
static const SrF64 SrPiF64		= 3.141592653589793;
static const SrF64 SrHalfPiF64	= 1.57079632679489661923;
static const SrF64 SrTwoPiF64	= 6.28318530717958647692;
static const SrF64 SrInvPiF64	= 0.31830988618379067154;
//we can get bad range checks if we use double prec consts to check single prec results.
static const SrF32 SrPiF32		= 3.141592653589793f;
static const SrF32 SrHalfPiF32	= 1.57079632679489661923f;
static const SrF32 SrTwoPiF32	= 6.28318530717958647692f;
static const SrF32 SrInvPiF32	= 0.31830988618379067154f;

/**
\brief Static class with stateless scalar math routines.
*/
class SrMath
	{
	public:

// Type conversion and rounding
		/**
		\brief Returns true if the two numbers are within eps of each other.
		*/
		SR_INLINE static bool equals(SrF32,SrF32,SrF32 eps);
		/**
		\brief Returns true if the two numbers are within eps of each other.
		*/
		SR_INLINE static bool equals(SrF64,SrF64,SrF64 eps);
		/**
		\brief The floor function returns a floating-point value representing the largest integer that is less than or equal to x.
		*/
		SR_INLINE static SrF32 floor(SrF32);
		/**
		\brief The floor function returns a floating-point value representing the largest integer that is less than or equal to x.
		*/
		SR_INLINE static SrF64 floor(SrF64);


		/**
		\brief The ceil function returns a single value representing the smallest integer that is greater than or equal to x. 
		*/
		SR_INLINE static SrF32 ceil(SrF32);
		/**
		\brief The ceil function returns a double value representing the smallest integer that is greater than or equal to x. 
		*/
		SR_INLINE static SrF64 ceil(SrF64);

		/**
		\brief Truncates the float to an integer.
		*/
		SR_INLINE static SrI32 trunc(SrF32);
		/**
		\brief Truncates the double precision float to an integer.
		*/
		SR_INLINE static SrI32 trunc(SrF64);
		/**
		\brief abs returns the absolute value of its argument. 
		*/
		SR_INLINE static SrF32 abs(SrF32);
		/**
		\brief abs returns the absolute value of its argument. 
		*/
		SR_INLINE static SrF64 abs(SrF64);
		/**
		\brief abs returns the absolute value of its argument. 
		*/
		SR_INLINE static SrI32 abs(SrI32);
		/**
		\brief sign returns the sign of its argument. The sign of zero is undefined.
		*/
		SR_INLINE static SrF32 sign(SrF32);
		/**
		\brief sign returns the sign of its argument. The sign of zero is undefined.
		*/
		SR_INLINE static SrF64 sign(SrF64);
		/**
		\brief sign returns the sign of its argument. The sign of zero is undefined.
		*/
		SR_INLINE static SrI32 sign(SrI32);
		/**
		\brief The return value is the greater of the two specified values. 
		*/
		SR_INLINE static SrF32 max(SrF32,SrF32);
		/**
		\brief The return value is the greater of the two specified values. 
		*/
		SR_INLINE static SrF64 max(SrF64,SrF64);
		/**
		\brief The return value is the greater of the two specified values. 
		*/
		SR_INLINE static SrI32 max(SrI32,SrI32);
		/**
		\brief The return value is the greater of the two specified values. 
		*/
		SR_INLINE static SrU32 max(SrU32,SrU32);
		/**
		\brief The return value is the greater of the two specified values. 
		*/
		SR_INLINE static SrU16 max(SrU16,SrU16);
		/**
		\brief The return value is the greater of the two specified values. 
		*/
		SR_INLINE static SrI16 max(SrI16,SrI16);
		/**
		\brief The return value is the lesser of the two specified values. 
		*/
		SR_INLINE static SrF32 min(SrF32,SrF32);
		/**
		\brief The return value is the lesser of the two specified values. 
		*/
		SR_INLINE static SrF64 min(SrF64,SrF64);
		/**
		\brief The return value is the lesser of the two specified values. 
		*/
		SR_INLINE static SrI32 min(SrI32,SrI32);
		/**
		\brief The return value is the lesser of the two specified values. 
		*/
		SR_INLINE static SrU32 min(SrU32,SrU32);
		/**
		\brief The return value is the lesser of the two specified values. 
		*/
		SR_INLINE static SrU16 min(SrU16,SrU16);
		/**
		\brief The return value is the lesser of the two specified values. 
		*/
		SR_INLINE static SrI16 min(SrI16,SrI16);
		
		/**
		\brief mod returns the floating-point remainder of x / y. 
		
		If the value of y is 0.0, mod returns a quiet NaN.
		*/
		SR_INLINE static SrF32 mod(SrF32 x, SrF32 y);
		/**
		\brief mod returns the floating-point remainder of x / y. 
		
		If the value of y is 0.0, mod returns a quiet NaN.
		*/
		SR_INLINE static SrF64 mod(SrF64 x, SrF64 y);

		/**
		\brief Clamps v to the range [hi,lo]
		*/
		SR_INLINE static SrF32 clamp(SrF32 v, SrF32 hi, SrF32 low);
		/**
		\brief Clamps v to the range [hi,lo]
		*/
		SR_INLINE static SrF64 clamp(SrF64 v, SrF64 hi, SrF64 low);
		/**
		\brief Clamps v to the range [hi,lo]
		*/
		SR_INLINE static SrU32 clamp(SrU32 v, SrU32 hi, SrU32 low);
		/**
		\brief Clamps v to the range [hi,lo]
		*/
		SR_INLINE static SrI32 clamp(SrI32 v, SrI32 hi, SrI32 low);

		//!powers
		/**
		\brief Square root.
		*/
		SR_INLINE static SrF32 sqrt(SrF32);
		/**
		\brief Square root.
		*/
		SR_INLINE static SrF64 sqrt(SrF64);
		
		/**
		\brief reciprocal square root.
		*/
		SR_INLINE static SrF32 recipSqrt(SrF32);
		/**
		\brief reciprocal square root.
		*/
		SR_INLINE static SrF64 recipSqrt(SrF64);
		
		/**
		\brief Calculates x raised to the power of y.
		*/
		SR_INLINE static SrF32 pow(SrF32 x, SrF32 y);
		/**
		\brief Calculates x raised to the power of y.
		*/
		SR_INLINE static SrF64 pow(SrF64 x, SrF64 y);
		
		
		/**
		\brief Calculates e^n
		*/
		SR_INLINE static SrF32 exp(SrF32);
		/**
		\brief Calculates e^n
		*/
		SR_INLINE static SrF64 exp(SrF64);
		
		/**
		\brief Calculates logarithms.
		*/
		SR_INLINE static SrF32 logE(SrF32);
		/**
		\brief Calculates logarithms.
		*/
		SR_INLINE static SrF64 logE(SrF64);
		/**
		\brief Calculates logarithms.
		*/
		SR_INLINE static SrF32 log2(SrF32);
		/**
		\brief Calculates logarithms.
		*/
		SR_INLINE static SrF64 log2(SrF64);
		/**
		\brief Calculates logarithms.
		*/
		SR_INLINE static SrF32 log10(SrF32);
		/**
		\brief Calculates logarithms.
		*/
		SR_INLINE static SrF64 log10(SrF64);

		//!trigonometry -- all angles are in radians.
		
		/**
		\brief Converts degrees to radians.
		*/
		SR_INLINE static SrF32 degToRad(SrF32);
		/**
		\brief Converts degrees to radians.
		*/
		SR_INLINE static SrF64 degToRad(SrF64);

		/**
		\brief Converts radians to degrees.
		*/
		SR_INLINE static SrF32 radToDeg(SrF32);
		/**
		\brief Converts radians to degrees.
		*/
		SR_INLINE static SrF64 radToDeg(SrF64);

		/**
		\brief Sine of an angle.

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF32 sin(SrF32);
		/**
		\brief Sine of an angle.

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF64 sin(SrF64);
		
		/**
		\brief Cosine of an angle.

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF32 cos(SrF32);
		/**
		\brief Cosine of an angle.

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF64 cos(SrF64);

		/**
		\brief Computes both the sin and cos.

		<b>Unit:</b> Radians
		*/
		SR_INLINE static void sinCos(SrF32, SrF32 & sin, SrF32 & cos);

		/**
		\brief Computes both the sin and cos.

		<b>Unit:</b> Radians
		*/
		SR_INLINE static void sinCos(SrF64, SrF64 & sin, SrF64 & cos);


		/**
		\brief Tangent of an angle.

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF32 tan(SrF32);
		/**
		\brief Tangent of an angle.
		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF64 tan(SrF64);
		
		/**
		\brief Arcsine.
		
		Returns angle between -PI/2 and PI/2 in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF32 asin(SrF32);
		/**
		\brief Arcsine.
		
		Returns angle between -PI/2 and PI/2 in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF64 asin(SrF64);
		
		/**
		\brief Arccosine.
		
		Returns angle between 0 and PI in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF32 acos(SrF32);
		
		/**
		\brief Arccosine.
		
		Returns angle between 0 and PI in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF64 acos(SrF64);
		
		/**
		\brief ArcTangent.
		
		Returns angle between -PI/2 and PI/2 in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF32 atan(SrF32);
		/**
		\brief ArcTangent.
		
		Returns angle between -PI/2 and PI/2 in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF64 atan(SrF64);

		/**
		\brief Arctangent of (x/y) with correct sign.
		
		Returns angle between -PI and PI in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF32 atan2(SrF32 x, SrF32 y);
		/**
		\brief Arctangent of (x/y) with correct sign.
		
		Returns angle between -PI and PI in radians

		<b>Unit:</b> Radians
		*/
		SR_INLINE static SrF64 atan2(SrF64 x, SrF64 y);

		//random numbers
		
		/**
		\brief uniform random number in [a,b]
		*/
		SR_INLINE static SrF32 rand(SrF32 a,SrF32 b);
		
		/**
		\brief uniform random number in [a,b]
		*/
		SR_INLINE static SrI32 rand(SrI32 a,SrI32 b);

		/**
		\brief hashing: hashes an array of n 32 bit values	to a 32 bit value.
		
		Because the output bits are uniformly distributed, the caller may mask
		off some of the bits to index into a hash table	smaller than 2^32.
		*/
		SR_INLINE static SrU32 hash(const SrU32 * array, SrU32 n);

		/**
		\brief hash32
		*/
		SR_INLINE static int hash32(int);

		/**
		\brief returns true if the passed number is a finite floating point number as opposed to INF, NAN, etc.
		*/
		SR_INLINE static bool isFinite(SrF32 x);
		
		/**
		\brief returns true if the passed number is a finite floating point number as opposed to INF, NAN, etc.
		*/
		SR_INLINE static bool isFinite(SrF64 x);
	};

/*
Many of these are just implemented as calls to the C lib right now,
but later we could replace some of them with some approximations or more
clever stuff.
*/
SR_INLINE bool SrMath::equals(SrF32 a,SrF32 b,SrF32 eps)
	{
	const SrF32 diff = SrMath::abs(a - b);
	return (diff < eps);
	}

SR_INLINE bool SrMath::equals(SrF64 a,SrF64 b,SrF64 eps)
	{
	const SrF64 diff = SrMath::abs(a - b);
	return (diff < eps);
	}

SR_INLINE SrF32 SrMath::floor(SrF32 a)
	{
	return ::floorf(a);
	}

SR_INLINE SrF64 SrMath::floor(SrF64 a)
	{
	return ::floor(a);
	}

SR_INLINE SrF32 SrMath::ceil(SrF32 a)
	{
	return ::ceilf(a);
	}

SR_INLINE SrF64 SrMath::ceil(SrF64 a)
	{
	return ::ceil(a);
	}

SR_INLINE SrI32 SrMath::trunc(SrF32 a)
	{
	return (SrI32) a;	// ### PT: this actually depends on FPU settings
	}

SR_INLINE SrI32 SrMath::trunc(SrF64 a)
	{
	return (SrI32) a;	// ### PT: this actually depends on FPU settings
	}

SR_INLINE SrF32 SrMath::abs(SrF32 a)
	{
	return ::fabsf(a);
	}

SR_INLINE SrF64 SrMath::abs(SrF64 a)
	{
	return ::fabs(a);
	}

SrI32 SrMath::abs(SrI32 a)
	{
	return ::abs(a);
	}

SR_INLINE SrF32 SrMath::sign(SrF32 a)
	{
	return abs(a)<SR_EPS_F32?0:((a > 0.0f) ? 1.0f : -1.0f);
	}

SR_INLINE SrF64 SrMath::sign(SrF64 a)
	{
		return abs(a)<SR_EPS_F64?0.0:((a > 0.0) ? 1.0 : -1.0);
	}

SR_INLINE SrI32 SrMath::sign(SrI32 a)
	{
		return a==0?0:((a >= 0) ? 1 : -1);
	}

SR_INLINE bool SrMath::isFinite(SrF32 f)
	{
		return (0 == ((_FPCLASS_SNAN | _FPCLASS_QNAN | _FPCLASS_NINF | _FPCLASS_PINF) & _fpclass(f) ));
	}
		
SR_INLINE bool SrMath::isFinite(SrF64 f)
	{
		return (0 == ((_FPCLASS_SNAN | _FPCLASS_QNAN | _FPCLASS_NINF | _FPCLASS_PINF) & _fpclass(f) ));
	}
SR_INLINE SrF32 SrMath::max(SrF32 a,SrF32 b)
{
	return (a < b) ? b : a;
}
SR_INLINE SrF64 SrMath::max(SrF64 a,SrF64 b)
{
	return (a < b) ? b : a;
}
SR_INLINE SrI32 SrMath::max(SrI32 a,SrI32 b)
	{
	return (a < b) ? b : a;
	}

SR_INLINE SrU32 SrMath::max(SrU32 a,SrU32 b)
	{
	return (a < b) ? b : a;
	}
SR_INLINE SrI16 SrMath::max(SrI16 a,SrI16 b)
{
	return (a < b) ? b : a;
}
SR_INLINE SrU16 SrMath::max(SrU16 a,SrU16 b)
	{
	return (a < b) ? b : a;
	}

SR_INLINE SrF32 SrMath::min(SrF32 a,SrF32 b)
{
	return (a < b) ? a : b;
}

SR_INLINE SrF64 SrMath::min(SrF64 a,SrF64 b)
{
	return (a < b) ? a : b;
}
SR_INLINE SrI32 SrMath::min(SrI32 a,SrI32 b)
{
	return (a < b) ? a : b;
}
SR_INLINE SrU32 SrMath::min(SrU32 a,SrU32 b)
	{
	return (a < b) ? a : b;
	}

SR_INLINE SrI16 SrMath::min(SrI16 a,SrI16 b)
	{
	return (a < b) ? a : b;
	}
SR_INLINE SrU16 SrMath::min(SrU16 a,SrU16 b)
{
	return (a < b) ? a : b;
}

SR_INLINE SrF32 SrMath::mod(SrF32 x, SrF32 y)
	{
	return (SrF32)::fmod(x,y);
	}

SR_INLINE SrF64 SrMath::mod(SrF64 x, SrF64 y)
	{
	return ::fmod(x,y);
	}

SR_INLINE SrF32 SrMath::clamp(SrF32 v, SrF32 hi, SrF32 low)
	{
	if (v > hi) 
		return hi;
	else if (v < low) 
		return low;
	else
		return v;
	}

SR_INLINE SrF64 SrMath::clamp(SrF64 v, SrF64 hi, SrF64 low)
	{
	if (v > hi) 
		return hi;
	else if (v < low) 
		return low;
	else
		return v;
	}

SR_INLINE SrU32 SrMath::clamp(SrU32 v, SrU32 hi, SrU32 low)
	{
	if (v > hi) 
		return hi;
	else if (v < low) 
		return low;
	else
		return v;
	}

SR_INLINE SrI32 SrMath::clamp(SrI32 v, SrI32 hi, SrI32 low)
	{
	if (v > hi) 
		return hi;
	else if (v < low) 
		return low;
	else
		return v;
	}

SR_INLINE SrF32 SrMath::recipSqrt(SrF32 a)
	{
	return 1.0f/::sqrtf(a);
	}

SR_INLINE SrF64 SrMath::recipSqrt(SrF64 a)
	{
	return 1.0/::sqrt(a);
	}


SR_INLINE SrF32 SrMath::pow(SrF32 x, SrF32 y)
	{
	return ::powf(x,y);
	}

SrF64 SrMath::pow(SrF64 x, SrF64 y)
	{
	return ::pow(x,y);
	}

SR_INLINE SrF32 SrMath::exp(SrF32 a)
	{
	return ::expf(a);
	}

SR_INLINE SrF64 SrMath::exp(SrF64 a)
	{
	return ::exp(a);
	}

SR_INLINE SrF32 SrMath::logE(SrF32 a)
	{
	return ::logf(a);
	}

SR_INLINE SrF64 SrMath::logE(SrF64 a)
	{
	return ::log(a);
	}

SR_INLINE SrF32 SrMath::log2(SrF32 a)
	{
	const SrF32 ln2 = (SrF32)0.693147180559945309417;
    return ::logf(a) / ln2;
	}

SR_INLINE SrF64 SrMath::log2(SrF64 a)
	{
	const SrF64 ln2 = (SrF64)0.693147180559945309417;
    return ::log(a) / ln2;
	}

SR_INLINE SrF32 SrMath::log10(SrF32 a)
	{
	return (SrF32)::log10(a);
	}

SR_INLINE SrF64 SrMath::log10(SrF64 a)
	{
	return ::log10(a);
	}

SR_INLINE SrF32 SrMath::degToRad(SrF32 a)
	{
	return (SrF32)0.01745329251994329547 * a;
	}

SR_INLINE SrF64 SrMath::degToRad(SrF64 a)
	{
	return (SrF64)0.01745329251994329547 * a;
	}

SR_INLINE SrF32 SrMath::radToDeg(SrF32 a)
	{
	return (SrF32)57.29577951308232286465 * a;
	}

SR_INLINE SrF64 SrMath::radToDeg(SrF64 a)
	{
	return (SrF64)57.29577951308232286465 * a;
	}

SR_INLINE SrF32 SrMath::sin(SrF32 a)
	{
	return ::sinf(a);
	}

SR_INLINE SrF64 SrMath::sin(SrF64 a)
	{
	return ::sin(a);
	}

SR_INLINE SrF32 SrMath::cos(SrF32 a)
	{
	return ::cosf(a);
	}

SR_INLINE SrF64 SrMath::cos(SrF64 a)
	{
	return ::cos(a);
	}

// Calling fsincos instead of fsin+fcos
SR_INLINE void SrMath::sinCos(SrF32 f, SrF32& s, SrF32& c)
	{
#if defined(WIN32) && !defined(_WIN64)
		SrF32 localCos, localSin;
		SrF32 local = f;
		_asm	fld		local
		_asm	fsincos
		_asm	fstp	localCos
		_asm	fstp	localSin
		c = localCos;
		s = localSin;
#else
		c = cosf(f);
		s = sinf(f);
#endif
	}

SR_INLINE void SrMath::sinCos(SrF64 a, SrF64 & s, SrF64 & c)
	{
	s = ::sin(a);
	c = ::cos(a);
	}

SR_INLINE SrF32 SrMath::tan(SrF32 a)
	{
	return ::tanf(a);
	}

SR_INLINE SrF64 SrMath::tan(SrF64 a)
	{
	return ::tan(a);
	}

SR_INLINE SrF32 SrMath::asin(SrF32 f)
	{
	// Take care of FPU inaccuracies
	if(f>=1.0f)	return (SrF32)SrHalfPiF32;
	if(f<=-1.0f)return -(SrF32)SrHalfPiF32;
				return ::asinf(f);
	}

SR_INLINE SrF64 SrMath::asin(SrF64 f)
	{
	// Take care of FPU inaccuracies
	if(f>=1.0)	return (SrF32)SrHalfPiF64;
	if(f<=-1.0)	return -(SrF32)SrHalfPiF64;
				return ::asin(f);
	}

SR_INLINE SrF32 SrMath::acos(SrF32 f)
	{
	// Take care of FPU inaccuracies
	if(f>=1.0f)	return 0.0f;
	if(f<=-1.0f)return (SrF32)SrPiF32;
				return ::acosf(f);
	}

SR_INLINE SrF64 SrMath::acos(SrF64 f)
	{
	// Take care of FPU inaccuracies
	if(f>=1.0)	return 0.0;
	if(f<=-1.0)	return (SrF64)SrPiF64;
				return ::acos(f);
	}

SR_INLINE SrF32 SrMath::atan(SrF32 a)
	{
	return ::atanf(a);
	}

SR_INLINE SrF64 SrMath::atan(SrF64 a)
	{
	return ::atan(a);
	}

SR_INLINE SrF32 SrMath::atan2(SrF32 x, SrF32 y)
	{
	return ::atan2f(x,y);
	}

SR_INLINE SrF64 SrMath::atan2(SrF64 x, SrF64 y)
	{
	return ::atan2(x,y);
	}

SR_INLINE SrF32 SrMath::rand(SrF32 a,SrF32 b)
	{
	const SrF32 r = (SrF32)::rand()/((SrF32)RAND_MAX+1);
	return r*(b-a) + a;
	}

SR_INLINE SrI32 SrMath::rand(SrI32 a,SrI32 b)
	{
	return a + (SrI32)(::rand()%(b-a));
	}

/*
--------------------------------------------------------------------
lookup2.c, by Bob Jenkins, December 1996, Public Domain.
hash(), hash2(), hash3, and mix() are externally useful functions.
Routines to test the hash are included if SELF_TEST is defined.
You can use this free for any purpose.  It has no warranty.
--------------------------------------------------------------------
--------------------------------------------------------------------
mix -- mix 3 32-bit values reversibly.
For every delta with one or two bit set, and the deltas of all three
  high bits or all three low bits, whether the original value of a,b,c
  is almost all zero or is uniformly distributed,
* If mix() is run forward or backward, at least 32 bits in a,b,c
  have at least 1/4 probability of changing.
* If mix() is run forward, every bit of c will change between 1/3 and
  2/3 of the time.  (Well, 22/100 and 78/100 for some 2-bit deltas.)
mix() was built out of 36 single-cycle latency instructions in a 
  structure that could supported 2x parallelism, like so:
      a -= b; 
      a -= c; x = (c>>13);
      b -= c; a ^= x;
      b -= a; x = (a<<8);
      c -= a; b ^= x;
      c -= b; x = (b>>13);
      ...
  Unfortunately, superscalar Pentiums and Sparcs can't take advantage 
  of that parallelism.  They've also turned some of those single-cycle
  latency instructions into multi-cycle latency instructions.  Still,
  this is the fastest good hash I could find.  There were about 2^^68
  to choose from.  I only looked at a billion or so.
--------------------------------------------------------------------
*/
#define SR_HASH_MIX(a,b,c) \
{ \
  a -= b; a -= c; a ^= (c>>13); \
  b -= c; b -= a; b ^= (a<<8); \
  c -= a; c -= b; c ^= (b>>13); \
  a -= b; a -= c; a ^= (c>>12);  \
  b -= c; b -= a; b ^= (a<<16); \
  c -= a; c -= b; c ^= (b>>5); \
  a -= b; a -= c; a ^= (c>>3);  \
  b -= c; b -= a; b ^= (a<<10); \
  c -= a; c -= b; c ^= (b>>15); \
}

/*
--------------------------------------------------------------------
 This works on all machines.  hash2() is identical to hash() on 
 little-endian machines, except that the length has to be measured
 in ub4s instead of bytes.  It is much faster than hash().  It 
 requires
 -- that the key be an array of ub4's, and
 -- that all your machines have the same endianness, and
 -- that the length be the number of ub4's in the key
--------------------------------------------------------------------
*/
SR_INLINE SrU32 SrMath::hash(const SrU32 *k, SrU32 length)
//register ub4 *k;        /* the key */
//register ub4  length;   /* the length of the key, in ub4s */
	{
	SrU32 a,b,c,len;

	/* Set up the internal state */
	len = length;
	a = b = 0x9e3779b9;  /* the golden ratio; an arbitrary value */
	c = 0;           /* the previous hash value */

	/*---------------------------------------- handle most of the key */
	while (len >= 3)
	{
	  a += k[0];
	  b += k[1];
	  c += k[2];
	  SR_HASH_MIX(a,b,c);
	  k += 3; len -= 3;
	}

	/*-------------------------------------- handle the last 2 ub4's */
	c += length;
	switch(len)              /* all the case statements fall through */
	{
	 /* c is reserved for the length */
	case 2 : b+=k[1];
	case 1 : a+=k[0];
	 /* case 0: nothing left to add */
	}
	SR_HASH_MIX(a,b,c);
	/*-------------------------------------------- report the result */
	return c;
	}
#undef SR_HASH_MIX

SR_INLINE int SrMath::hash32(int key)
	{
	key += ~(key << 15);
	key ^=  (key >> 10);
	key +=  (key << 3);
	key ^=  (key >> 6);
	key += ~(key << 11);
	key ^=  (key >> 16);
	return key;
	}

SR_INLINE SrF32 SrMath::sqrt(SrF32 a)
	{
		return (SrF32)::sqrt(a);
	}
SR_INLINE SrF64 SrMath::sqrt(SrF64 a)
	{
		return (SrF64)::sqrt(a);
	}


/** @} */
#endif

spacer

Leave a reply