开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > Windows编程 > 界面编程 > 查看源码
gltl.h
资源名称：profuis288_freeware.zip　[点击查看]
上传用户：sesekoo
上传日期：2020-07-18
资源大小：21543k
文件大小：46k
源码类别：

界面编程

开发平台：

Visual C++

gltl.h：源码内容
  1. #if (!defined __GLTL_H)
  2. #define __GLTL_H
  4. // disable warning C4201
  5. #pragma warning( push )
  6. #pragma warning( disable : 4201 )
  8. namespace gltl
  9. {
  11. template < class _num >
  12. class ntraits;
  13. template < class _num, class _ntr >
  14. class vector3;
  15. template < class _num, class _ntr >
  16. class vector4;
  17. template < class _num, class _ntr >
  18. class matrix;
  20. #include <math.h>
  22. #if (defined _DEBUG)
  23. #if (defined ASSERT)
  24. #define __gltl_assert__(__x__) ASSERT(__x__)
  25. #elif (defined ATLASSERT)
  26. #define __gltl_assert__(__x__) ATLASSERT(__x__)
  27. #else
  28. #include <assert.h>
  29. #define __gltl_assert__(__x__) assert(__x__)
  30. #endif
  31. #else
  32. #define __gltl_assert__(__x__)
  33. #endif // _DEBUG
  35. template < class _num >
  36. class ntraits
  37. {
  38. public:
  40. typedef const _num _numc;
  41. typedef _num & _numr;
  42. typedef const _num & _numrc;
  43. typedef _num * _nump;
  44. typedef const _num * _numpc;
  46. static _num adjust_angle_d(
  47. _numc _angle_degrees,
  48. long _l_adjust_prec = 1000L
  49. )
  50. {
  51. long _ang = long( _angle_degrees * ((_num)(_l_adjust_prec)) );
  53. long _modx360 = 360L * _l_adjust_prec;
  54. // long _modx180 = _modx360 >> 1; // _modx360/2
  55. _ang %= _modx360;
  56. if( _ang < 0L )
  57. _ang = _modx360 + _ang;
  58. __gltl_assert__( _ang >= 0L );
  59. // if( _ang > _modx180 )
  60. // _ang = _ang%_modx180 - _modx180;
  61. _num _ret_val = (_num)(_ang);
  62. _ret_val /= (_num)(_l_adjust_prec);
  63. return _ret_val;
  64. }
  66. static _numc get_pi()
  67. {
  68. return ((_num)(3.1415926535897932384626433832795));
  69. }
  71. static _numc get_safe_min()
  72. {
  73. return ((_num)(0.000001));
  74. }
  76. static _numc get_def_epsilon()
  77. {
  78. return ((_num)(0.0001));
  79. }
  81. static _num r2d( _numc _angle_radians )
  82. {
  83. return
  84. adjust_angle_d( 
  85. ( _angle_radians * ((_num)180.0) )
  86. / get_pi()
  87. );
  88. }
  90. static _num d2r( _numc _angle_degrees )
  91. {
  92. return
  93. ( adjust_angle_d(_angle_degrees) * get_pi() )
  94. / ((_num)180.0)
  95. ;
  96. }
  98. static _num abs( _numc _val )
  99. {
  100. return (_val >= (_num)(0.0)) ? _val : (-_val);
  101. }
  103. static _num sin( _numc _val )
  104. {
  105. return ( (_num) ::sin(_val) );
  106. }
  107. static _num cos( _numc _val )
  108. {
  109. return ( (_num) ::cos(_val) );
  110. }
  111. static _num tan( _numc _val )
  112. {
  113. return ( (_num) ::tan(_val) );
  114. }
  116. static _num asin( _numc _val )
  117. {
  118. return ( (_num) ::asin(_val) );
  119. }
  120. static _num acos( _numc _val )
  121. {
  122. return ( (_num) ::acos(_val) );
  123. }
  124. static _num atan( _numc _val )
  125. {
  126. return ( (_num) ::atan(_val) );
  127. }
  129. static _num atan2( _numc _y, _numc _x )
  130. {
  131. return ( (_num) ::atan2( _y, _x ) );
  132. }
  134. static _num sqrt( _numc _val )
  135. {
  136. return ( (_num) ::sqrt(_val) );
  137. }
  139. };
  141. template < class _num, class _ntr >
  142. class vector3
  143. {
  144. public:
  146. typedef const _num _numc;
  147. typedef _num & _numr;
  148. typedef const _num & _numrc;
  149. typedef _num * _nump;
  150. typedef const _num * _numpc;
  152. typedef vector3<_num,_ntr> _v3t;
  153. typedef vector3<_num,_ntr> & _v3rt;
  154. typedef const vector3<_num,_ntr> & _v3rct;
  156. typedef vector4<_num,_ntr> _v4t;
  157. typedef vector4<_num,_ntr> & _v4rt;
  158. typedef const vector4<_num,_ntr> & _v4rct;
  160. typedef matrix<_num,_ntr> _mt;
  161. typedef matrix<_num,_ntr> & _mrt;
  162. typedef const matrix<_num,_ntr> & _mrct;
  164. union 
  165. {
  166. struct { _num x, y, z; };
  167. struct { _num a, b, c; };
  168. _num arr[3];
  169. };
  171. vector3(
  172. _numc _x = ((_num)0.0),
  173. _numc _y = ((_num)0.0),
  174. _numc _z = ((_num)0.0)
  175. )
  176. : x( _x )
  177. , y( _y )
  178. , z( _z )
  179. {
  180. }
  182. void load_vector(
  183. _num _x = ((_num)0.0),
  184. _num _y = ((_num)0.0),
  185. _num _z = ((_num)0.0)
  186. )
  187. {
  188. x = _x;
  189. y = _y;
  190. z = _z;
  191. }
  193. void translate(
  194. _num _x = ((_num)0.0),
  195. _num _y = ((_num)0.0),
  196. _num _z = ((_num)0.0)
  197. )
  198. {
  199. x += _x;
  200. y += _y;
  201. z += _z;
  202. }
  204. void scale(
  205. _num _x = ((_num)1.0),
  206. _num _y = ((_num)1.0),
  207. _num _z = ((_num)1.0)
  208. )
  209. {
  210. x *= _x;
  211. y *= _y;
  212. z *= _z;
  213. }
  215. _num get_length_sq() const
  216. {
  217. return ( (_num) (x*x + y*y + z*z) );
  218. }
  220. _num get_length() const
  221. {
  222. return _ntr::sqrt( get_length_sq() );
  223. }
  225. _mt get_as_scale() const;
  226. _mt get_as_translation() const;
  227. _mt get_as_rotation_xyz() const;
  228. _mt get_as_rotation_quaternion() const;
  230. _v3rt normalize() 
  231. {
  232. _num _len = get_length();
  233. if( ( _len < _ntr::get_safe_min() ) && ( _len > (- _ntr::get_safe_min()) ) )
  234. _len = 1;
  235. x /= _len;
  236. y /= _len;
  237. z /= _len;
  238. return (*this);
  239. }
  241. static inline void stat_multiplication(
  242. _mrct _mtx,
  243. _v3rct _v,
  244. _v3rt _ret_val
  245. );
  246. static inline _v3t stat_multiplication(
  247. _mrct _mtx,
  248. _v3rct _v
  249. );
  251. _numr operator () ( int _i )
  252. {
  253. __gltl_assert__( ( 0 <= _i ) && ( _i <= 3 ) );
  254. return *(((_nump)&x) + _i);
  255. }
  257. _numr operator [] ( int _i ) 
  258. {
  259. __gltl_assert__( ( 0 <= _i ) && ( _i <= 2 ) );
  260. return *(((_nump)&x) + _i);
  261. }
  262. _numrc operator [] ( int _i ) const
  263. {
  264. __gltl_assert__( ( 0 <= _i ) && ( _i <= 2 ) );
  265. return *(((_nump)&x) + _i);
  266. }
  268. bool operator == ( _v3rct _v ) const
  269. {
  270. return ( x==_v.x && y==_v.y && z==_v.z ) ? true : false;
  271. }
  272. bool operator != ( _v3rct _v ) const
  273. {
  274. return ( x==_v.x && y==_v.y && z==_v.z ) ? false : true;
  275. }
  277. _v3rt operator = ( _v3rct _v )
  278. {
  279. x = _v.x;
  280. y = _v.y;
  281. z = _v.z;
  282. return (*this);
  283. }
  284. _v3rt operator = ( _v4rct );
  286. _v3rt operator *= ( _numc _s ) 
  287. {
  288. x *= _s;
  289. y *= _s;
  290. z *= _s;
  291. return (*this);
  292. }
  293. _v3rt operator *= ( _mrct _mtx ) 
  294. {
  295. stat_multiplication( _mtx, *this, *this );
  296. return (*this);
  297. }
  298. _v3rt operator += ( _v3rct _v ) 
  299. {
  300. x += _v.x;
  301. y += _v.y;
  302. z += _v.z;
  303. return (*this);
  304. }
  305. _v3rt operator -= ( _v3rct _v ) 
  306. {
  307. x -= _v.x;
  308. y -= _v.y;
  309. z -= _v.z;
  310. return (*this);
  311. }
  313. };
  315. template < class _num, class _ntr >
  316. inline vector3<_num,_ntr> operator * (
  317. const vector3<_num,_ntr> & _v,
  318. const _num _s
  319. ) 
  320. {
  321. vector3<_num,_ntr> _ret_val;
  322. _ret_val.x = _v.x * _s;
  323. _ret_val.y = _v.y * _s;
  324. _ret_val.z = _v.z * _s;
  325. return _ret_val;
  326. }
  328. template < class _num, class _ntr >
  329. inline vector3<_num,_ntr> operator * (
  330. const _num _s,
  331. const vector3<_num,_ntr> & _v
  332. ) 
  333. {
  334. vector3<_num,_ntr> _ret_val;
  335. _ret_val.x = _v.x * _s;
  336. _ret_val.y = _v.y * _s;
  337. _ret_val.z = _v.z * _s;
  338. return _ret_val;
  339. }
  341. template < class _num, class _ntr >
  342. inline _num operator * (
  343. const vector3<_num,_ntr> & _v1,
  344. const vector3<_num,_ntr> & _v2
  345. ) 
  346. {
  347. return ( _v1.x*_v2.x + _v1.y*_v2.y + _v1.z*_v2.z );
  348. }
  350. template < class _num, class _ntr >
  351. inline vector3<_num,_ntr> operator ^ (
  352. const vector3<_num,_ntr> & _v1,
  353. const vector3<_num,_ntr> & _v2
  354. ) 
  355. {
  356. vector3<_num,_ntr> _ret_val;
  357. _ret_val.x = _v1.y*_v2.z - _v1.z*_v2.y;
  358. _ret_val.y = _v1.z*_v2.x - _v1.x*_v2.z;
  359. _ret_val.z = _v1.x*_v2.y - _v1.y*_v2.x;
  360. return _ret_val;
  361. }
  363. template < class _num, class _ntr >
  364. inline vector3<_num,_ntr> operator + (
  365. const vector3<_num,_ntr> & _v1,
  366. const vector3<_num,_ntr> & _v2
  367. ) 
  368. {
  369. vector3<_num,_ntr> _ret_val;
  370. _ret_val.x = _v1.x + _v2.x;
  371. _ret_val.y = _v1.y + _v2.y;
  372. _ret_val.z = _v1.z + _v2.z;
  373. return _ret_val;
  374. }
  376. template < class _num, class _ntr >
  377. inline vector3<_num,_ntr> operator - (
  378. const vector3<_num,_ntr> & _v1,
  379. const vector3<_num,_ntr> & _v2
  380. ) 
  381. {
  382. vector3<_num,_ntr> _ret_val;
  383. _ret_val.x = _v1.x - _v2.x;
  384. _ret_val.y = _v1.y - _v2.y;
  385. _ret_val.z = _v1.z - _v2.z;
  386. return _ret_val;
  387. }
  389. template < class _num, class _ntr >
  390. inline vector3<_num,_ntr> operator - (
  391. const vector3<_num,_ntr> & _v
  392. ) 
  393. {
  394. vector3<_num,_ntr> _ret_val;
  395. _ret_val.x = - _v.x;
  396. _ret_val.y = - _v.y;
  397. _ret_val.z = - _v.z;
  398. return _ret_val;
  399. }
  401. template < class _num, class _ntr >
  402. inline vector3<_num,_ntr> operator + (
  403. const vector3<_num,_ntr> & _v
  404. ) 
  405. {
  406. return _v;
  407. }
  409. template < class _num, class _ntr >
  410. inline vector3<_num,_ntr> operator ~ (
  411. const vector3<_num,_ntr> & _v
  412. ) 
  413. {
  414. _num _len = _v.get_length();
  415. vector3<_num,_ntr> _ret_val;
  416. _ret_val.x = _v.x / _len;
  417. _ret_val.y = _v.y / _len;
  418. _ret_val.z = _v.z / _len;
  419. return _ret_val;
  420. }
  422. template < class _num, class _ntr >
  423. class vector4
  424. {
  425. public:
  427. typedef const _num _numc;
  428. typedef _num & _numr;
  429. typedef const _num & _numrc;
  430. typedef _num * _nump;
  431. typedef const _num * _numpc;
  433. typedef vector3<_num,_ntr> _v3t;
  434. typedef vector3<_num,_ntr> & _v3rt;
  435. typedef const vector3<_num,_ntr> & _v3rct;
  437. typedef vector4<_num,_ntr> _v4t;
  438. typedef vector4<_num,_ntr> & _v4rt;
  439. typedef const vector4<_num,_ntr> & _v4rct;
  441. typedef matrix<_num,_ntr> _mt;
  442. typedef matrix<_num,_ntr> & _mrt;
  443. typedef const matrix<_num,_ntr> & _mrct;
  445. union 
  446. {
  447. struct { _num x, y, z, w; };
  448. struct { _num a, b, c, d; };
  449. _num arr[4];
  450. };
  452. vector4(
  453. _numc _x = ((_num)0.0),
  454. _numc _y = ((_num)0.0),
  455. _numc _z = ((_num)0.0),
  456. _numc _w = ((_num)0.0)
  457. )
  458. : x( _x )
  459. , y( _y )
  460. , z( _z )
  461. , w( _w )
  462. {
  463. }
  465. void load_vector(
  466. _num _x = ((_num)0.0),
  467. _num _y = ((_num)0.0),
  468. _num _z = ((_num)0.0),
  469. _num _w = ((_num)0.0)
  470. )
  471. {
  472. x = _x;
  473. y = _y;
  474. z = _z;
  475. w = _w;
  476. }
  478. void translate(
  479. _num _x = ((_num)0.0),
  480. _num _y = ((_num)0.0),
  481. _num _z = ((_num)0.0),
  482. _num _w = ((_num)0.0)
  483. )
  484. {
  485. x += _x;
  486. y += _y;
  487. z += _z;
  488. w += _w;
  489. }
  491. void scale(
  492. _num _x = ((_num)1.0),
  493. _num _y = ((_num)1.0),
  494. _num _z = ((_num)1.0),
  495. _num _w = ((_num)1.0)
  496. )
  497. {
  498. x *= _x;
  499. y *= _y;
  500. z *= _z;
  501. w *= _w;
  502. }
  504. _num get_length_sq() const
  505. {
  506. return ( (_num) (x*x + y*y + z*z + w*w) );
  507. }
  509. _num get_length() const
  510. {
  511. return _ntr::sqrt( get_length_sq() );
  512. }
  514. _v4rt normalize() 
  515. {
  516. _num _len = get_length();
  517. if( ( _len < _ntr::get_safe_min() ) && ( _len > (- _ntr::get_safe_min()) ) )
  518. _len = 1;
  519. x /= _len;
  520. y /= _len;
  521. z /= _len;
  522. w /= _len;
  523. return (*this);
  524. }
  526. static inline _v4t stat_multiplication(
  527. _mrct _mtx,
  528. _v4rct _v
  529. );
  530. static inline void stat_multiplication(
  531. _mrct _mtx,
  532. _v4rct _v,
  533. _v4rt _ret_val
  534. );
  536. _numr operator () ( int _i )
  537. {
  538. __gltl_assert__( ( 0 <= _i ) && ( _i <= 3 ) );
  539. return *(((_nump)&x) + _i);
  540. }
  542. _numr operator [] ( int _i ) 
  543. {
  544. __gltl_assert__( ( 0 <= _i ) && ( _i <= 3 ) );
  545. return *(((_nump)&x) + _i);
  546. }
  547. _numrc operator [] ( int _i ) const
  548. {
  549. __gltl_assert__( ( 0 <= _i ) && ( _i <= 3 ) );
  550. return *(((_nump)&x) + _i);
  551. }
  553. bool operator == ( _v4rct _v ) const
  554. {
  555. return ( x==_v.x && y==_v.y && z==_v.z && w==_v.w ) ? true : false;
  556. }
  557. bool operator != ( _v4rct _v ) const
  558. {
  559. return ( x==_v.x && y==_v.y && z==_v.z && w==_v.w ) ? false : true;
  560. }
  562. _v4rt operator = ( _v4rct _v )
  563. {
  564. x = _v.x;
  565. y = _v.y;
  566. z = _v.z;
  567. w = _v.w;
  568. return (*this);
  569. }
  570. _v4rt operator = ( _v3rct _v )
  571. {
  572. x = _v.x;
  573. y = _v.y;
  574. z = _v.z;
  575. w = ((_num)0.0);
  576. return (*this);
  577. }
  579. _v4rt operator *= ( _numc _s ) 
  580. {
  581.  x *= _s;
  582.  y *= _s;
  583.  z *= _s;
  584.  w *= _s;
  585. return (*this);
  586. }
  587. _v4rt operator *= ( _mrct _mtx ) 
  588. {
  589. stat_multiplication( _mtx, *this, *this );
  590. return (*this);
  591. }
  592. _v4rt operator += ( _v4rct _v ) 
  593. {
  594. x += _v.x;
  595. y += _v.y;
  596. z += _v.z;
  597. w += _v.w;
  598. return (*this);
  599. }
  600. _v4rt operator -= ( _v4rct _v ) 
  601. {
  602. x -= _v.x;
  603. y -= _v.y;
  604. z -= _v.z;
  605. w -= _v.w;
  606. return (*this);
  607. }
  609. };
  611. template < class _num, class _ntr >
  612. inline vector4<_num,_ntr> operator * (
  613. const vector4<_num,_ntr> & _v,
  614. const _num _s
  615. ) 
  616. {
  617. vector4<_num,_ntr> _ret_val;
  618. _ret_val.x = _v.x * _s;
  619. _ret_val.y = _v.y * _s;
  620. _ret_val.z = _v.z * _s;
  621. _ret_val.w = _v.w * _s;
  622. return _ret_val;
  623. }
  625. template < class _num, class _ntr >
  626. inline vector4<_num,_ntr> operator * (
  627. const _num _s,
  628. const vector4<_num,_ntr> & _v
  629. ) 
  630. {
  631. vector4<_num,_ntr> _ret_val;
  632. _ret_val.x = _v.x * _s;
  633. _ret_val.y = _v.y * _s;
  634. _ret_val.z = _v.z * _s;
  635. _ret_val.w = _v.w * _s;
  636. return _ret_val;
  637. }
  639. template < class _num, class _ntr >
  640. inline _num operator * (
  641. const vector4<_num,_ntr> & _v1,
  642. const vector4<_num,_ntr> & _v2
  643. ) 
  644. {
  645. return ( _v1.x*_v2.x + _v1.y*_v2.y + _v1.z*_v2.z + _v1.w*_v2.w );
  646. }
  648. template < class _num, class _ntr >
  649. inline vector4<_num,_ntr> operator + (
  650. const vector4<_num,_ntr> & _v1,
  651. const vector4<_num,_ntr> & _v2
  652. ) 
  653. {
  654. vector4<_num,_ntr> _ret_val;
  655. _ret_val.x = _v1.x + _v2.x;
  656. _ret_val.y = _v1.y + _v2.y;
  657. _ret_val.z = _v1.z + _v2.z;
  658. _ret_val.w = _v1.w + _v2.w;
  659. return _ret_val;
  660. }
  662. template < class _num, class _ntr >
  663. inline vector4<_num,_ntr> operator - (
  664. const vector4<_num,_ntr> & _v1,
  665. const vector4<_num,_ntr> & _v2
  666. ) 
  667. {
  668. vector4<_num,_ntr> _ret_val;
  669. _ret_val.x = _v1.x - _v2.x;
  670. _ret_val.y = _v1.y - _v2.y;
  671. _ret_val.z = _v1.z - _v2.z;
  672. _ret_val.w = _v1.w - _v2.w;
  673. return _ret_val;
  674. }
  676. template < class _num, class _ntr >
  677. inline vector4<_num,_ntr> operator - (
  678. const vector4<_num,_ntr> & _v
  679. ) 
  680. {
  681. vector4<_num,_ntr> _ret_val;
  682. _ret_val.x = - _v.x;
  683. _ret_val.y = - _v.y;
  684. _ret_val.z = - _v.z;
  685. _ret_val.w = - _v.w;
  686. return _ret_val;
  687. }
  689. template < class _num, class _ntr >
  690. inline vector4<_num,_ntr> operator + (
  691. const vector4<_num,_ntr> & _v
  692. ) 
  693. {
  694. return _v;
  695. }
  697. template < class _num, class _ntr >
  698. inline vector4<_num,_ntr> operator ~ (
  699. const vector4<_num,_ntr> & _v
  700. ) 
  701. {
  702. _num _len = _v.get_length();
  703. vector4<_num,_ntr> _ret_val;
  704. _ret_val.x = _v.x / _len;
  705. _ret_val.y = _v.y / _len;
  706. _ret_val.z = _v.z / _len;
  707. _ret_val.w = _v.w / _len;
  708. return _ret_val;
  709. }
  711. template < class _num, class _ntr >
  712. inline vector4<_num,_ntr> operator ^ (
  713. const vector4<_num,_ntr> & _v1,
  714. const vector4<_num,_ntr> & _v2
  715. ) 
  716. {
  717. vector4<_num,_ntr> _ret_val;
  718. _ret_val.x = _v1.y*_v2.z - _v1.z*_v2.y;
  719. _ret_val.y = _v1.z*_v2.x - _v1.x*_v2.z;
  720. _ret_val.z = _v1.x*_v2.y - _v1.y*_v2.x;
  721. _ret_val.w = ((_num)0.0);
  722. return _ret_val;
  723. }
  725. template < class _num, class _ntr >
  726. inline vector3<_num,_ntr> & vector3<_num,_ntr>::operator = (
  727. const vector4<_num,_ntr> & _v
  728. )
  729. {
  730. x = _v.x;
  731. y = _v.y;
  732. z = _v.z;
  733. return (*this);
  734. }
  736. template < class _num, class _ntr >
  737. class quaternion : public vector4<_num,_ntr>
  738. {
  739. public:
  741. typedef quaternion<_num,_ntr> _q4t;
  742. typedef quaternion<_num,_ntr> & _q4rt;
  743. typedef const quaternion<_num,_ntr> & _q4rct;
  745. quaternion()
  746. {
  747. load_quaternion();
  748. }
  750. quaternion( _q4rct _q )
  751. {
  752. set_quaternion( _q );
  753. }
  755. quaternion( _numc _x, _numc _y, _numc _z )
  756. {
  757. set_quaternion( _x, _y, _z );
  758. }
  760. quaternion( _v3rct _v )
  761. {
  762. set_quaternion( _v );
  763. }
  765. quaternion( _numc _angle, _numc _x, _numc _y, _numc _z )
  766. {
  767. set_quaternion( _angle, _x, _y, _z );
  768. }
  770. quaternion( _numc _mtx[3][3] )
  771. {
  772. _num _tr, _s, _q[4];
  773. int _i, _j, _k, _nxt_val[3] = {1, 2, 0};
  774. _tr = _mtx[0][0] + _mtx[1][1] + _mtx[2][2];
  775. if( _tr > 0.0 ) 
  776. {
  777. _s = _ntr::sqrt( _tr + ((_num)1.0) );
  778. w  = _s / ((_num)2.0);
  779. _s = ((_num)0.5) / _s;
  780. x  = (_mtx[1][2] - _mtx[2][1]) * _s;
  781. y  = (_mtx[2][0] - _mtx[0][2]) * _s;
  782. z  = (_mtx[0][1] - _mtx[1][0]) * _s;
  783. } 
  784. else 
  785. {
  786.      _i = 0;
  787. if( _mtx[1][1] > _mtx[0][0] )
  788. _i = 1;
  789. if( _mtx[2][2] > _mtx[_i][_i] )
  790. _i = 2;
  791. _j = _nxt_val[_i];
  792. _k = _nxt_val[_j];
  793. _s =
  794. _ntr::sqrt(
  795. ( _mtx[_i][_i] - ( _mtx[_j][_j] + _mtx[_k][_k] ) )
  796. + 1.0
  797. );
  798. _q[_i] = _s * ((_num)0.5);
  799. if( _s != ((_num)0.0) )
  800. _s = ((_num)0.5) / _s;
  801. _q[3]  = (_mtx[_j][_k] - _mtx[_k][_j]) * _s;
  802. _q[_j] = (_mtx[_i][_j] + _mtx[_j][_i]) * _s;
  803. _q[_k] = (_mtx[_i][_k] + _mtx[_k][_i]) * _s;
  804. x = _q[0];
  805. y = _q[1];
  806. z = _q[2];
  807. w = _q[3];
  808. }
  809. }
  811. quaternion( _mrt _mtx );
  813. void load_quaternion(
  814. _numc _x = ((_num)0.0),
  815. _numc _y = ((_num)0.0),
  816. _numc _z = ((_num)0.0),
  817. _numc _w = ((_num)1.0)
  818. )
  819. {
  820. x = _x;
  821. y = _y;
  822. z = _z;
  823. w = _w;
  824. }
  826. void set_quaternion( _num _angle, _num _x, _num _y, _num _z )
  827. {
  828. _numc _fct = _x*_x+_y*_y+_z*_z;
  829. __gltl_assert__( _fct != 0 );
  830. _numc _scale_by( ((_num)1.0) / _ntr::sqrt(_fct) );
  831. _x = _x * _scale_by;
  832. _y = _y * _scale_by;
  833. _z = _z * _scale_by;
  834. w = _ntr::cos( _angle / ((_num)2.0) );
  835. _numc _sin_half( _ntr::sin( _angle / ((_num)2.0) ) );
  836. x = _x * _sin_half;
  837. y = _y * _sin_half;
  838. z = _z * _sin_half;
  839. }
  840. void set_quaternion( _numc _x, _numc _y, _numc _z )
  841. {
  842. _q4t _x_q( _x, ((_num)1.0), ((_num)0.0), ((_num)0.0) );
  843. _q4t _y_q( _y, ((_num)0.0), ((_num)1.0), ((_num)0.0) );
  844. _q4t _z_q( _z, ((_num)0.0), ((_num)0.0), ((_num)1.0) );
  845. set_quaternion( _x_q );
  846. (*this) *= _y_q;
  847. (*this) *= _z_q;
  848. }
  849. void set_quaternion( _v3rct _v )
  850. {
  851. set_quaternion( _v.x, _v.y, _v.z );
  852. }
  853. void set_quaternion( _q4rct _q )
  854. {
  855. x = _q.x;
  856. y = _q.y;
  857. z = _q.z;
  858. w = _q.w;
  859. }
  861. void normalize()
  862. {
  863. _num _fct = get_length_sq();
  864. __gltl_assert__( _fct != 0 );
  865. _num _scale_by( ((_num)1.0) / _ntr::sqrt(_fct) );
  866. x = x * _scale_by;
  867. y = y * _scale_by;
  868. z = z * _scale_by;
  869. w = w * _scale_by;
  870. }
  872. void get_as_matrix( _mrt _mtx );
  873. _mt get_as_matrix() const;
  875. void get_axis_angle( _numr axis_x, _numr axis_y, _numr axis_z, _numr _rot_angle ) const
  876. {
  877. _numc _vec_len2 = x*x + y*y + z*z;
  878. if( _vec_len2 < _ntr::get_def_epsilon() )
  879. {
  880. axis_x = ((_num)1.0);
  881. axis_y = ((_num)0.0);
  882. axis_z = ((_num)0.0);
  883. _rot_angle = ((_num)0.0);
  884. }
  885. else
  886. {
  887. _numc _len_inv = ((_num)1.0) / _ntr::sqrt(_vec_len2);
  888. axis_x = x * _len_inv;
  889. axis_y = y * _len_inv;
  890. axis_z = z * _len_inv;
  891. _rot_angle = ((_num)2.0) * _ntr::acos(w);
  892. }
  893. }
  894. void get_axis_angle( _v3rt _v, _numr _rot_angle ) const
  895. {
  896. get_axis_angle( _v.x, _v.y, _v.z, _rot_angle );
  897. }
  899. void get_lt_ln( _numr _angle_lat, _numr _angle_long ) const
  900. {
  901. _num _x, _y, _z;
  902. get_direction_vector( _x, _y, _z );
  903. _angle_lat = _ntr::asin( _y );
  904. if( _ntr::abs(_y) > ( ((_num)1.0) - _ntr::get_def_epsilon() ) )
  905. _angle_long = ((_num)0.0);
  906. else
  907. {
  908. if( _ntr::abs(_z) < _ntr::get_def_epsilon() )
  909. _angle_long = _x > ((_num)0.0)
  910. ? ( _ntr::get_pi() / ((_num)2.0) )
  911. : ( _ntr::get_pi() * ((_num)3.0) / ((_num)2.0) )
  912. ;
  913. else
  914. {
  915. if( _z >= ((_num)0.0) )
  916. _angle_long = _ntr::atan( _x / _z );
  917. else
  918. _angle_long = _ntr::get_pi() + _ntr::atan( _x / _z );
  920. if( _angle_long < ((_num)0.0) )
  921. _angle_long += _ntr::get_pi() * ((_num)2.0);
  922. }
  923. }
  924. }
  926. void get_direction_vector( _numr _x, _numr _y, _numr _z )
  927. {
  928. normalize();
  929. _x = ((_num)2.0) * ( x * z - w * y );
  930. _y = ((_num)2.0) * ( y * z + w * x );
  931. _z = ((_num)1.0) - ((_num)2.0) * ( x * x + y * y );
  932. }
  933. void get_direction_vector( _v3rt _v )
  934. {
  935. get_direction_vector( _v.x, _v.y, _v.z );
  936. }
  938. static _q4t stat_multiplication( _q4rct _q1, _q4rct _q2 )
  939. {
  940. _q4t _q;
  941. _q.x =
  942.   _q2.w*_q1.x
  943. + _q2.x*_q1.w
  944. + _q2.y*_q1.z
  945. - _q2.z*_q1.y;
  946. _q.y =
  947.   _q2.w*_q1.y
  948. - _q2.x*_q1.z
  949. + _q2.y*_q1.w
  950. + _q2.z*_q1.x;
  951. _q.z =
  952.   _q2.w*_q1.z
  953. + _q2.x*_q1.y
  954. - _q2.y*_q1.x
  955. + _q2.z*_q1.w;
  956. _q.w =
  957.   _q2.w*_q1.w
  958. - _q2.x*_q1.x
  959. - _q2.y*_q1.y
  960. - _q2.z*_q1.z;
  961. return _q;
  962. }
  964. _q4rt operator *= ( _q4rct _q2 )
  965. {
  966. _q4t _holder( *this );
  967. (*this) = stat_multiplication( _holder, _q2 );
  968. return (*this);
  969. }
  971. };
  973. template < class _num, class _ntr >
  974. class matrix
  975. {
  976. public:
  978. typedef const _num _numc;
  979. typedef _num & _numr;
  980. typedef const _num & _numrc;
  981. typedef _num * _nump;
  982. typedef const _num * _numpc;
  984. typedef vector3<_num,_ntr> _v3t;
  985. typedef vector3<_num,_ntr> & _v3rt;
  986. typedef const vector3<_num,_ntr> & _v3rct;
  988. typedef vector4<_num,_ntr> _v4t;
  989. typedef vector4<_num,_ntr> & _v4rt;
  990. typedef const vector4<_num,_ntr> & _v4rct;
  992. typedef quaternion<_num,_ntr> _q4t;
  993. typedef quaternion<_num,_ntr> & _q4rt;
  994. typedef const quaternion<_num,_ntr> & _q4rct;
  996. typedef matrix<_num,_ntr> _mt;
  997. typedef matrix<_num,_ntr> & _mrt;
  998. typedef const matrix<_num,_ntr> & _mrct;
  1000. union
  1001. {
  1002. struct
  1003. {
  1004. _num
  1005. m11, m21, m31, m41,
  1006. m12, m22, m32, m42,
  1007. m13, m23, m33, m43,
  1008. m14, m24, m34, m44;
  1009. };
  1010. _num arr[16];
  1011. _num _xx[4][4];
  1012. };
  1014. matrix(
  1015. _num src11 = ((_num)1.0), _num src21 = ((_num)0.0), _num src31 = ((_num)0.0), _num src41 = ((_num)0.0),
  1016. _num src12 = ((_num)0.0), _num src22 = ((_num)1.0), _num src32 = ((_num)0.0), _num src42 = ((_num)0.0),
  1017. _num src13 = ((_num)0.0), _num src23 = ((_num)0.0), _num src33 = ((_num)1.0), _num src43 = ((_num)0.0),
  1018. _num src14 = ((_num)0.0), _num src24 = ((_num)0.0), _num src34 = ((_num)0.0), _num src44 = ((_num)1.0)
  1019. )
  1020. : m14(src14), m13(src13), m12(src12), m11(src11)
  1021. , m24(src24), m23(src23), m22(src22), m21(src21)
  1022. , m34(src34), m33(src33), m32(src32), m31(src31)
  1023. , m44(src44), m43(src43), m42(src42), m41(src41)
  1024. {
  1025. }
  1027. matrix( _nump p_src )
  1028. : m14(p_src[ 3]), m13(p_src[ 2]), m12(p_src[ 1]), m11(p_src[ 0])
  1029. , m24(p_src[ 7]), m23(p_src[ 6]), m22(p_src[ 5]), m21(p_src[ 4])
  1030. , m34(p_src[11]), m33(p_src[10]), m32(p_src[ 9]), m31(p_src[ 8])
  1031. , m44(p_src[15]), m43(p_src[14]), m42(p_src[13]), m41(p_src[12])
  1032. {
  1033. }
  1035. matrix( _num _mtx[3][3] )
  1036. : m14(_mtx[0][0]),  m13(_mtx[0][1]),  m12(_mtx[0][2]),  m11(((_num)0.0))
  1037. , m24(_mtx[1][0]),  m23(_mtx[1][1]),  m22(_mtx[1][2]),  m21(((_num)0.0))
  1038. , m34(_mtx[2][0]),  m33(_mtx[2][1]),  m32(_mtx[2][2]),  m31(((_num)0.0))
  1039. , m44(((_num)0.0)), m43(((_num)0.0)), m42(((_num)0.0)), m41(((_num)1.0))
  1040. {
  1041. }
  1043. _numr get_item_ref_xy( int _i = 0, int _j = 0 )
  1044. {
  1045. __gltl_assert__( (0<=_i) && (_i<=3) && (0<=_j) && (_j<=3) );
  1046. return *(((_nump)&m11) + (_j<<2)+_i);
  1047. }
  1048. _numrc get_item_ref_xy( int _i = 0, int _j = 0 ) const
  1049. {
  1050. __gltl_assert__( (0<=_i) && (_i<=3) && (0<=_j) && (_j<=3) );
  1051. return *(((_nump)&m11) + (_j<<2)+_i);
  1052. }
  1053. _nump get_item_ptr_xy( int _i = 0, int _j = 0 )
  1054. {
  1055. __gltl_assert__( (0<=_i) && (_i<=3) && (0<=_j) && (_j<=3) );
  1056. return (((_nump)&m11) + (_j<<2)+_i);
  1057. }
  1058. _numpc get_item_ptr_xy( int _i = 0, int _j = 0 ) const
  1059. {
  1060. __gltl_assert__( (0<=_i) && (_i<=3) && (0<=_j) && (_j<=3) );
  1061. return (((_nump)&m11) + (_j<<2)+_i);
  1062. }
  1063. _numr operator () ( int _i, int _j )
  1064. {
  1065. return get_item_ref_xy( _i, _j );
  1066. }
  1067. _numrc operator () ( int _i, int _j ) const
  1068. {
  1069. return get_item_ref_xy( _i, _j );
  1070. }
  1071. _numr get_item_ref_yx( int _i = 0, int _j = 0 )
  1072. {
  1073. return get_item_ref_xy( _j, _i );
  1074. }
  1075. _numrc get_item_ref_yx( int _i = 0, int _j = 0 ) const
  1076. {
  1077. return get_item_ref_xy( _j, _i );
  1078. }
  1079. _nump get_item_ptr_yx( int _i = 0, int _j = 0 )
  1080. {
  1081. return get_item_ptr_xy( _j, _i );
  1082. }
  1083. _numpc get_item_ptr_yx( int _i = 0, int _j = 0 ) const
  1084. {
  1085. return get_item_ptr_xy( _j, _i );
  1086. }
  1088. bool operator == ( _mrct _mtx ) const
  1089. {
  1090. return (
  1091.    m11 == _mtx.m11 && m12 == _mtx.m12 && m13 == _mtx.m13 && m14 == _mtx.m14
  1092. && m21 == _mtx.m21 && m22 == _mtx.m22 && m23 == _mtx.m23 && m24 == _mtx.m24
  1093. && m31 == _mtx.m31 && m32 == _mtx.m32 && m33 == _mtx.m33 && m34 == _mtx.m34
  1094. && m41 == _mtx.m41 && m42 == _mtx.m42 && m43 == _mtx.m43 && m44 == _mtx.m44
  1095. ) ? true : false;
  1096. }
  1097. bool operator != ( _mrct _mtx ) const
  1098. {
  1099. return (
  1100.    m11 == _mtx.m11 && m12 == _mtx.m12 && m13 == _mtx.m13 && m14 == _mtx.m14
  1101. && m21 == _mtx.m21 && m22 == _mtx.m22 && m23 == _mtx.m23 && m24 == _mtx.m24
  1102. && m31 == _mtx.m31 && m32 == _mtx.m32 && m33 == _mtx.m33 && m34 == _mtx.m34
  1103. && m41 == _mtx.m41 && m42 == _mtx.m42 && m43 == _mtx.m43 && m44 == _mtx.m44
  1104. ) ? false : true;
  1105. }
  1107. _mrt operator = ( _mrct _mtx ) 
  1108. {
  1109. m11 = _mtx.m11; m12 = _mtx.m12; m13 = _mtx.m13; m14 = _mtx.m14;
  1110. m21 = _mtx.m21; m22 = _mtx.m22; m23 = _mtx.m23; m24 = _mtx.m24;
  1111. m31 = _mtx.m31; m32 = _mtx.m32; m33 = _mtx.m33; m34 = _mtx.m34;
  1112. m41 = _mtx.m41; m42 = _mtx.m42; m43 = _mtx.m43; m44 = _mtx.m44;
  1113. return (*this);
  1114. }
  1116. static void stat_multiplication(
  1117. _mrct _mtx1,
  1118. _mrct _mtx2,
  1119. _mrt _ret_val
  1120. ) 
  1121. {
  1122. _mt _ret_val_prep;
  1123. _ret_val_prep.m11 = _mtx1.m11*_mtx2.m11 + _mtx1.m12*_mtx2.m21 + _mtx1.m13*_mtx2.m31 + _mtx1.m14*_mtx2.m41;
  1124. _ret_val_prep.m12 = _mtx1.m11*_mtx2.m12 + _mtx1.m12*_mtx2.m22 + _mtx1.m13*_mtx2.m32 + _mtx1.m14*_mtx2.m42;
  1125. _ret_val_prep.m13 = _mtx1.m11*_mtx2.m13 + _mtx1.m12*_mtx2.m23 + _mtx1.m13*_mtx2.m33 + _mtx1.m14*_mtx2.m43;
  1126. _ret_val_prep.m14 = _mtx1.m11*_mtx2.m14 + _mtx1.m12*_mtx2.m24 + _mtx1.m13*_mtx2.m34 + _mtx1.m14*_mtx2.m44;
  1127. _ret_val_prep.m21 = _mtx1.m21*_mtx2.m11 + _mtx1.m22*_mtx2.m21 + _mtx1.m23*_mtx2.m31 + _mtx1.m24*_mtx2.m41;
  1128. _ret_val_prep.m22 = _mtx1.m21*_mtx2.m12 + _mtx1.m22*_mtx2.m22 + _mtx1.m23*_mtx2.m32 + _mtx1.m24*_mtx2.m42;
  1129. _ret_val_prep.m23 = _mtx1.m21*_mtx2.m13 + _mtx1.m22*_mtx2.m23 + _mtx1.m23*_mtx2.m33 + _mtx1.m24*_mtx2.m43;
  1130. _ret_val_prep.m24 = _mtx1.m21*_mtx2.m14 + _mtx1.m22*_mtx2.m24 + _mtx1.m23*_mtx2.m34 + _mtx1.m24*_mtx2.m44;
  1131. _ret_val_prep.m31 = _mtx1.m31*_mtx2.m11 + _mtx1.m32*_mtx2.m21 + _mtx1.m33*_mtx2.m31 + _mtx1.m34*_mtx2.m41;
  1132. _ret_val_prep.m32 = _mtx1.m31*_mtx2.m12 + _mtx1.m32*_mtx2.m22 + _mtx1.m33*_mtx2.m32 + _mtx1.m34*_mtx2.m42;
  1133. _ret_val_prep.m33 = _mtx1.m31*_mtx2.m13 + _mtx1.m32*_mtx2.m23 + _mtx1.m33*_mtx2.m33 + _mtx1.m34*_mtx2.m43;
  1134. _ret_val_prep.m34 = _mtx1.m31*_mtx2.m14 + _mtx1.m32*_mtx2.m24 + _mtx1.m33*_mtx2.m34 + _mtx1.m34*_mtx2.m44;
  1135. _ret_val_prep.m41 = _mtx1.m41*_mtx2.m11 + _mtx1.m42*_mtx2.m21 + _mtx1.m43*_mtx2.m31 + _mtx1.m44*_mtx2.m41;
  1136. _ret_val_prep.m42 = _mtx1.m41*_mtx2.m12 + _mtx1.m42*_mtx2.m22 + _mtx1.m43*_mtx2.m32 + _mtx1.m44*_mtx2.m42;
  1137. _ret_val_prep.m43 = _mtx1.m41*_mtx2.m13 + _mtx1.m42*_mtx2.m23 + _mtx1.m43*_mtx2.m33 + _mtx1.m44*_mtx2.m43;
  1138. _ret_val_prep.m44 = _mtx1.m41*_mtx2.m14 + _mtx1.m42*_mtx2.m24 + _mtx1.m43*_mtx2.m34 + _mtx1.m44*_mtx2.m44;
  1139. // for( int _i = 0; _i < 4; _i++ )
  1140. // {
  1141. // for( int _j = 0; _j < 4; _j++ )
  1142. // {
  1143. // _ret_val_prep._xx[_i][_j] = 0;
  1144. // for( int _k = 0; _k < 4; _k++ )
  1145. // {
  1146. // _ret_val_prep._xx[_i][_j] +=
  1147. // _mtx2._xx[_i][_k] * _mtx1._xx[_k][_j];
  1148. // }
  1149. // }
  1150. // }
  1151. _ret_val = _ret_val_prep;
  1152. }
  1154. static _mt stat_multiplication(
  1155. _mrct _mtx1,
  1156. _mrct _mtx2
  1157. ) 
  1158. {
  1159. _mt _ret_val;
  1160. _ret_val.m11 = _mtx1.m11*_mtx2.m11 + _mtx1.m12*_mtx2.m21 + _mtx1.m13*_mtx2.m31 + _mtx1.m14*_mtx2.m41;
  1161. _ret_val.m12 = _mtx1.m11*_mtx2.m12 + _mtx1.m12*_mtx2.m22 + _mtx1.m13*_mtx2.m32 + _mtx1.m14*_mtx2.m42;
  1162. _ret_val.m13 = _mtx1.m11*_mtx2.m13 + _mtx1.m12*_mtx2.m23 + _mtx1.m13*_mtx2.m33 + _mtx1.m14*_mtx2.m43;
  1163. _ret_val.m14 = _mtx1.m11*_mtx2.m14 + _mtx1.m12*_mtx2.m24 + _mtx1.m13*_mtx2.m34 + _mtx1.m14*_mtx2.m44;
  1164. _ret_val.m21 = _mtx1.m21*_mtx2.m11 + _mtx1.m22*_mtx2.m21 + _mtx1.m23*_mtx2.m31 + _mtx1.m24*_mtx2.m41;
  1165. _ret_val.m22 = _mtx1.m21*_mtx2.m12 + _mtx1.m22*_mtx2.m22 + _mtx1.m23*_mtx2.m32 + _mtx1.m24*_mtx2.m42;
  1166. _ret_val.m23 = _mtx1.m21*_mtx2.m13 + _mtx1.m22*_mtx2.m23 + _mtx1.m23*_mtx2.m33 + _mtx1.m24*_mtx2.m43;
  1167. _ret_val.m24 = _mtx1.m21*_mtx2.m14 + _mtx1.m22*_mtx2.m24 + _mtx1.m23*_mtx2.m34 + _mtx1.m24*_mtx2.m44;
  1168. _ret_val.m31 = _mtx1.m31*_mtx2.m11 + _mtx1.m32*_mtx2.m21 + _mtx1.m33*_mtx2.m31 + _mtx1.m34*_mtx2.m41;
  1169. _ret_val.m32 = _mtx1.m31*_mtx2.m12 + _mtx1.m32*_mtx2.m22 + _mtx1.m33*_mtx2.m32 + _mtx1.m34*_mtx2.m42;
  1170. _ret_val.m33 = _mtx1.m31*_mtx2.m13 + _mtx1.m32*_mtx2.m23 + _mtx1.m33*_mtx2.m33 + _mtx1.m34*_mtx2.m43;
  1171. _ret_val.m34 = _mtx1.m31*_mtx2.m14 + _mtx1.m32*_mtx2.m24 + _mtx1.m33*_mtx2.m34 + _mtx1.m34*_mtx2.m44;
  1172. _ret_val.m41 = _mtx1.m41*_mtx2.m11 + _mtx1.m42*_mtx2.m21 + _mtx1.m43*_mtx2.m31 + _mtx1.m44*_mtx2.m41;
  1173. _ret_val.m42 = _mtx1.m41*_mtx2.m12 + _mtx1.m42*_mtx2.m22 + _mtx1.m43*_mtx2.m32 + _mtx1.m44*_mtx2.m42;
  1174. _ret_val.m43 = _mtx1.m41*_mtx2.m13 + _mtx1.m42*_mtx2.m23 + _mtx1.m43*_mtx2.m33 + _mtx1.m44*_mtx2.m43;
  1175. _ret_val.m44 = _mtx1.m41*_mtx2.m14 + _mtx1.m42*_mtx2.m24 + _mtx1.m43*_mtx2.m34 + _mtx1.m44*_mtx2.m44;
  1176. // for( int _i = 0; _i < 4; _i++ )
  1177. // {
  1178. // for( int _j = 0; _j < 4; _j++ )
  1179. // {
  1180. // _ret_val._xx[_i][_j] = 0;
  1181. // for( int _k = 0; _k < 4; _k++ )
  1182. // {
  1183. // _ret_val._xx[_i][_j] +=
  1184. // _mtx2._xx[_i][_k] * _mtx1._xx[_k][_j];
  1185. // }
  1186. // }
  1187. // }
  1188. return _ret_val;
  1189. }
  1191. _mrt operator *= ( _q4rct _q ); 
  1192. _mrt operator *= ( _mrct _mtx ) 
  1193. {
  1194. stat_multiplication( *this, _mtx, *this );
  1195. return (*this);
  1196. }
  1197. _mrt operator *= ( _numc _v ) 
  1198. {
  1199. for( int _i = 0; _i < 16; _i++ )
  1200. *(((_num*)&m11)+_i) = *(((_num*)&m11)+_i) * _v;
  1201. return (*this);
  1202. }
  1203. _mrt operator += ( _mrct _mtx )
  1204. {
  1205. for( int _i = 0; _i < 16; _i++ )
  1206. *(((_num*)&m11)+_i) = *(((_num*)&m11)+_i) + *(((_num*)&_mtx.m11)+_i);
  1207. return (*this);
  1208. }
  1209. _mrt operator -= ( _mrct _mtx ) 
  1210. {
  1211. for( int _i = 0; _i < 16; _i++ )
  1212. *(((_num*)&m11)+_i) = *(((_num*)&m11)+_i) - *(((_num*)&_mtx.m11)+_i);
  1213. return (*this);
  1214. }
  1216. void load_transposition() 
  1217. {
  1218. _num _swap_val;
  1219. _swap_val = m12; m12 = m21; m21 = _swap_val;
  1220. _swap_val = m13; m13 = m31; m31 = _swap_val;
  1221. _swap_val = m14; m14 = m41; m41 = _swap_val;
  1222. _swap_val = m23; m23 = m32; m32 = _swap_val;
  1223. _swap_val = m24; m24 = m42; m42 = _swap_val;
  1224. _swap_val = m34; m34 = m43; m43 = _swap_val;
  1225. }
  1227. _num load_inversion()
  1228. {
  1229. _num _dest[16], _swap[12], _src[16];
  1230. for( int _i = 0; _i < 4; _i++ ) 
  1231. {
  1232. _src[_i]      = *(((_nump)&m11) + (_i<<2));
  1233. _src[_i +  4] = *(((_nump)&m11) + (_i<<2) + 1);
  1234. _src[_i +  8] = *(((_nump)&m11) + (_i<<2) + 2);
  1235. _src[_i + 12] = *(((_nump)&m11) + (_i<<2) + 3);
  1236. }
  1237. _swap[ 0] = _src[10] * _src[15];
  1238. _swap[ 1] = _src[11] * _src[14];
  1239. _swap[ 2] = _src[ 9] * _src[15];
  1240. _swap[ 3] = _src[11] * _src[13];
  1241. _swap[ 4] = _src[ 9] * _src[14];
  1242. _swap[ 5] = _src[10] * _src[13];
  1243. _swap[ 6] = _src[ 8] * _src[15];
  1244. _swap[ 7] = _src[11] * _src[12];
  1245. _swap[ 8] = _src[ 8] * _src[14];
  1246. _swap[ 9] = _src[10] * _src[12];
  1247. _swap[10] = _src[ 8] * _src[13];
  1248. _swap[11] = _src[ 9] * _src[12];
  1249. _dest[ 0] = _swap[0]*_src[5] + _swap[3]*_src[6] + _swap[ 4]*_src[7];
  1250. _dest[ 0]-= _swap[1]*_src[5] + _swap[2]*_src[6] + _swap[ 5]*_src[7];
  1251. _dest[ 1] = _swap[1]*_src[4] + _swap[6]*_src[6] + _swap[ 9]*_src[7];
  1252. _dest[ 1]-= _swap[0]*_src[4] + _swap[7]*_src[6] + _swap[ 8]*_src[7];
  1253. _dest[ 2] = _swap[2]*_src[4] + _swap[7]*_src[5] + _swap[10]*_src[7];
  1254. _dest[ 2]-= _swap[3]*_src[4] + _swap[6]*_src[5] + _swap[11]*_src[7];
  1255. _dest[ 3] = _swap[5]*_src[4] + _swap[8]*_src[5] + _swap[11]*_src[6];
  1256. _dest[ 3]-= _swap[4]*_src[4] + _swap[9]*_src[5] + _swap[10]*_src[6];
  1257. _dest[ 4] = _swap[1]*_src[1] + _swap[2]*_src[2] + _swap[ 5]*_src[3];
  1258. _dest[ 4]-= _swap[0]*_src[1] + _swap[3]*_src[2] + _swap[ 4]*_src[3];
  1259. _dest[ 5] = _swap[0]*_src[0] + _swap[7]*_src[2] + _swap[ 8]*_src[3];
  1260. _dest[ 5]-= _swap[1]*_src[0] + _swap[6]*_src[2] + _swap[ 9]*_src[3];
  1261. _dest[ 6] = _swap[3]*_src[0] + _swap[6]*_src[1] + _swap[11]*_src[3];
  1262. _dest[ 6]-= _swap[2]*_src[0] + _swap[7]*_src[1] + _swap[10]*_src[3];
  1263. _dest[ 7] = _swap[4]*_src[0] + _swap[9]*_src[1] + _swap[10]*_src[2];
  1264. _dest[ 7]-= _swap[5]*_src[0] + _swap[8]*_src[1] + _swap[11]*_src[2];
  1265. _swap[ 0] = _src[2]*_src[7];
  1266. _swap[ 1] = _src[3]*_src[6];
  1267. _swap[ 2] = _src[1]*_src[7];
  1268. _swap[ 3] = _src[3]*_src[5];
  1269. _swap[ 4] = _src[1]*_src[6];
  1270. _swap[ 5] = _src[2]*_src[5];
  1271. _swap[ 6] = _src[0]*_src[7];
  1272. _swap[ 7] = _src[3]*_src[4];
  1273. _swap[ 8] = _src[0]*_src[6];
  1274. _swap[ 9] = _src[2]*_src[4];
  1275. _swap[10] = _src[0]*_src[5];
  1276. _swap[11] = _src[1]*_src[4];
  1277. _dest[ 8] = _swap[ 0]*_src[13] + _swap[ 3]*_src[14] + _swap[ 4]*_src[15];
  1278. _dest[ 8]-= _swap[ 1]*_src[13] + _swap[ 2]*_src[14] + _swap[ 5]*_src[15];
  1279. _dest[ 9] = _swap[ 1]*_src[12] + _swap[ 6]*_src[14] + _swap[ 9]*_src[15];
  1280. _dest[ 9]-= _swap[ 0]*_src[12] + _swap[ 7]*_src[14] + _swap[ 8]*_src[15];
  1281. _dest[10] = _swap[ 2]*_src[12] + _swap[ 7]*_src[13] + _swap[10]*_src[15];
  1282. _dest[10]-= _swap[ 3]*_src[12] + _swap[ 6]*_src[13] + _swap[11]*_src[15];
  1283. _dest[11] = _swap[ 5]*_src[12] + _swap[ 8]*_src[13] + _swap[11]*_src[14];
  1284. _dest[11]-= _swap[ 4]*_src[12] + _swap[ 9]*_src[13] + _swap[10]*_src[14];
  1285. _dest[12] = _swap[ 2]*_src[10] + _swap[ 5]*_src[11] + _swap[ 1]*_src[ 9];
  1286. _dest[12]-= _swap[ 4]*_src[11] + _swap[ 0]*_src[ 9] + _swap[ 3]*_src[10];
  1287. _dest[13] = _swap[ 8]*_src[11] + _swap[ 0]*_src[ 8] + _swap[ 7]*_src[10];
  1288. _dest[13]-= _swap[ 6]*_src[10] + _swap[ 9]*_src[11] + _swap[ 1]*_src[ 8];
  1289. _dest[14] = _swap[ 6]*_src[ 9] + _swap[11]*_src[11] + _swap[ 3]*_src[ 8];
  1290. _dest[14]-= _swap[10]*_src[11] + _swap[ 2]*_src[ 8] + _swap[ 7]*_src[ 9];
  1291. _dest[15] = _swap[10]*_src[10] + _swap[ 4]*_src[ 8] + _swap[ 9]*_src[ 9];
  1292. _dest[15]-= _swap[ 8]*_src[ 9] + _swap[11]*_src[10] + _swap[ 5]*_src[ 8];
  1293. _num _det1 =
  1294.   _src[0]*_dest[0]
  1295. + _src[1]*_dest[1]
  1296. + _src[2]*_dest[2]
  1297. + _src[3]*_dest[3];
  1298. _num _det2 = ((_num)1.0) / _det1;
  1299. for( int _j = 0; _j < 16; _j++ )
  1300. {
  1301. _dest[_j] *= _det2;
  1302. *(((_nump)&m11) + _j) = _dest[_j];
  1303. }
  1304. return _det1;
  1305. }
  1307. _num get_determinant() 
  1308. {
  1309. _num _src[16], _swap[12], _dest[4];
  1310. for( int _i = 0; _i < 16; _i++ )
  1311. _src[_i] = *(((_nump)&m11) + _i);
  1312. _swap[ 0] = _src[10] * _src[15];
  1313. _swap[ 1] = _src[11] * _src[14];
  1314. _swap[ 2] = _src[ 9] * _src[15];
  1315. _swap[ 3] = _src[11] * _src[13];
  1316. _swap[ 4] = _src[ 9] * _src[14];
  1317. _swap[ 5] = _src[10] * _src[13];
  1318. _swap[ 6] = _src[ 8] * _src[15];
  1319. _swap[ 7] = _src[11] * _src[12];
  1320. _swap[ 8] = _src[ 8] * _src[14];
  1321. _swap[ 9] = _src[10] * _src[12];
  1322. _swap[10] = _src[ 8] * _src[13];
  1323. _swap[11] = _src[ 9] * _src[12];
  1324. _dest[ 0] = _swap[0]*_src[5] + _swap[3]*_src[6] + _swap[ 4]*_src[7];
  1325. _dest[ 0]-= _swap[1]*_src[5] + _swap[2]*_src[6] + _swap[ 5]*_src[7];
  1326. _dest[ 1] = _swap[1]*_src[4] + _swap[6]*_src[6] + _swap[ 9]*_src[7];
  1327. _dest[ 1]-= _swap[0]*_src[4] + _swap[7]*_src[6] + _swap[ 8]*_src[7];
  1328. _dest[ 2] = _swap[2]*_src[4] + _swap[7]*_src[5] + _swap[10]*_src[7];
  1329. _dest[ 2]-= _swap[3]*_src[4] + _swap[6]*_src[5] + _swap[11]*_src[7];
  1330. _dest[ 3] = _swap[5]*_src[4] + _swap[8]*_src[5] + _swap[11]*_src[6];
  1331. _dest[ 3]-= _swap[4]*_src[4] + _swap[9]*_src[5] + _swap[10]*_src[6];
  1332. _num _det =
  1333.   _src[0]*_dest[0]
  1334. + _src[1]*_dest[1]
  1335. + _src[2]*_dest[2]
  1336. + _src[3]*_dest[3];
  1337. return _det;
  1338. }
  1340. _v3t get_angles()
  1341. {
  1342. _num yy, xx, zz, _k, rd;
  1343. rd = _ntr::sqrt( m13*m13 + m23*m23 + m33*m33 );
  1344. _k = ( ((_num)1.0) - _ntr::get_safe_min() ) * rd;
  1345. if( m23 > _k )
  1346. {
  1347. zz = ((_num)0.0);
  1348. xx = - _ntr::get_pi() / ((_num)2.0);
  1349. yy = _ntr::atan2( -m12, m11 );
  1350. }
  1351. else if( m23 < (-_k) )
  1352. {
  1353. zz = ((_num)0.0);
  1354. xx = _ntr::get_pi() / ((_num)2.0);
  1355. yy = _ntr::atan2( m12, m11 );
  1356. }
  1357. else
  1358. {
  1359. if( rd == 0)
  1360. yy = xx = zz = ((_num)0.0);
  1361. else
  1362. {
  1363. xx =  - _ntr::asin( m23 / rd );
  1364. yy =    _ntr::atan2( m13, m33 );
  1365. zz =    _ntr::atan2( m21, m22 );
  1366. }
  1367. }
  1368. return _v3t( xx, yy, zz );
  1369. }
  1371. _v3t get_position()
  1372. {
  1373. return _v3t( m14, m24, m34 );
  1374. }
  1376. void load_zero()
  1377. {
  1378.   m11 = m12 = m13 = m14
  1379. = m21 = m22 = m23 = m24
  1380. = m31 = m32 = m33 = m34
  1381. = m41 = m42 = m43 = m44
  1382. = ((_num)0.0);
  1383. }
  1385. void load_identity()
  1386. {
  1387. load_zero();
  1388. m11 = m22 = m33 = m44 = ((_num)1.0);
  1389. }
  1391. void load_translation(
  1392. _numc dx,
  1393. _numc dy,
  1394. _numc dz
  1395. )
  1396. {
  1397. load_identity();
  1398. m14 = dx;
  1399. m24 = dy;
  1400. m34 = dz;
  1401. }
  1402. void load_translation( _v3rct _v )
  1403. {
  1404. load_identity();
  1405. m14 = _v.x;
  1406. m24 = _v.y;
  1407. m34 = _v.z;
  1408. }
  1410. void load_rotation_x( _numc _angle_radians )
  1411. {
  1412. _num _c = _ntr::cos(_angle_radians), _s = _ntr::sin(_angle_radians);
  1413. load_identity();
  1414. m22 = + _c;
  1415. m23 = - _s;
  1416. m32 = + _s;
  1417. m33 = + _c;
  1418. }
  1419. void load_rotation_y( _numc _angle_radians )
  1420. {
  1421. _num _c = _ntr::cos(_angle_radians), _s = _ntr::sin(_angle_radians);
  1422. load_identity();
  1423. m11 = + _c;
  1424. m13 = + _s;
  1425. m31 = - _s;
  1426. m33 = + _c;
  1427. }
  1428. void load_rotation_z( _numc _angle_radians )
  1429. {
  1430. _num _c = _ntr::cos(_angle_radians), _s = _ntr::sin(_angle_radians);
  1431. load_identity();
  1432. m11 = + _c;
  1433. m12 = - _s;
  1434. m21 = + _s;
  1435. m22 = + _c;
  1436. }
  1437. void load_rotation_xyz( _numc _x, _numc _y, _numc _z )
  1438. {
  1439. _mt mtx_rotation_x, mtx_rotation_y, mtx_rotation_z;
  1440. mtx_rotation_x.load_rotation_x(_x);
  1441. mtx_rotation_y.load_rotation_y(_y);
  1442. mtx_rotation_z.load_rotation_z(_z);
  1443. (*this) = mtx_rotation_x * mtx_rotation_y * mtx_rotation_z;
  1444. }
  1445. void load_rotation_xyz( _v3rct _v )
  1446. {
  1447. load_rotation_xyz( _v.x, _v.y, _v.z );
  1448. }
  1450. void load_scale( _numc _x, _numc _y, _numc _z )
  1451. {
  1452. load_identity();
  1453. m11 = _x;
  1454. m22 = _y;
  1455. m33 = _z;
  1456. } 
  1457. void load_scale( _v3rct _v )
  1458. {
  1459. load_identity();
  1460. m11 = _v.x;
  1461. m22 = _v.y;
  1462. m33 = _v.z;
  1463. } 
  1464. void load_scale( _numc _val )
  1465. {
  1466. load_identity();
  1467. m11 = m22 = m33 = _val;
  1468. }
  1470. void load_perspective(
  1471. _num fovy,
  1472. _num aspect,
  1473. _num znear,
  1474. _num zfar
  1475. )
  1476. {
  1477. load_identity();
  1478. _num _ctan_fov =
  1479. ((_num)1.0)
  1480. /
  1481. ( _ntr::tan( fovy / ((_num)2.0)) );
  1482. m11 =   _ctan_fov / aspect;
  1483. m22 =   _ctan_fov;
  1484. m33 = - (zfar + znear) / (zfar - znear);
  1485. m34 = - ((_num)2.0) * (zfar * znear) / ( zfar - znear );
  1486. m43 = - ((_num)1.0);
  1487. m44 =   ((_num)0.0);
  1488. }
  1490. void load_rotation_about_vector(
  1491. _numc _angle,
  1492. _v3rct _vec
  1493. )
  1494. {
  1495. _v3t _v( _vec );
  1496. _v.normalize();
  1497. _num _x = _v._x, _y = _v._y, _z = _v._z;
  1498. _num _c = _ntr::cos(_angle);
  1499. _num _s = _ntr::sin(_angle);
  1500. _num xx = _x * _x;
  1501. _num xy = _y * _x;
  1502. _num xz = _z * _x;
  1503. _num yy = _y * _y;
  1504. _num yz = _y * _z;
  1505. _num zz = _z * _z;
  1506. _num xs = _x * _s;
  1507. _num ys = _y * _s;
  1508. _num zs = _z * _s;
  1509. _num _1c = 1 - _c;
  1510. m11 = xx*_1c+_c;
  1511. m12 = xy*_1c+zs;
  1512. m13 = xz*_1c-ys;
  1513. m14 = 0;
  1514. m21 = xy*_1c-zs;
  1515. m22 = yy*_1c+_c;
  1516. m23 = yz*_1c+xs;
  1517. m24 = 0;
  1518. m31 = xz*_1c+ys;
  1519. m32 = yz*_1c-xs;
  1520. m33 = zz*_1c+_c;
  1521. m34 = 0;
  1522. m41 = 0;
  1523. m42 = 0;
  1524. m43 = 0;
  1525. m44 = 1;
  1526. }
  1527. void load_rotation_about_vector(
  1528. _numc _angle,
  1529. _numc _x,
  1530. _numc _y,
  1531. _numc _z
  1532. )
  1533. {
  1534. load_rotation_about_vector(
  1535. _angle,
  1536. _v3t( _x, _y, _z )
  1537. );
  1538. }
  1539. void load_rotation_about_vector(
  1540. _numc _angle,
  1541. _v4rct _vec
  1542. )
  1543. {
  1544. load_rotation_about_vector(
  1545. _angle,
  1546. _v3t(_vec.x,_vec.y,_vec.z)
  1547. );
  1548. }
  1550. void load_reflection(
  1551. _numc _a,
  1552. _numc _b,
  1553. _numc _c,
  1554. _numc _d
  1555. )
  1556. {
  1557. get_item_ref_yx( 0, 0 ) = _numc(1.0) - _numc(2.0) * _a * _a;
  1558. get_item_ref_yx( 1, 0 ) =            - _numc(2.0) * _a * _b;
  1559. get_item_ref_yx( 2, 0 ) =            - _numc(2.0) * _a * _c;
  1560. get_item_ref_yx( 3, 0 ) =            - _numc(2.0) * _d * _a;
  1561. get_item_ref_yx( 0, 1 ) =            - _numc(2.0) * _b * _a;
  1562. get_item_ref_yx( 1, 1 ) = _numc(1.0) - _numc(2.0) * _b * _b;
  1563. get_item_ref_yx( 2, 1 ) =            - _numc(2.0) * _b * _c;
  1564. get_item_ref_yx( 3, 1 ) =            - _numc(2.0) * _d * _b;
  1565. get_item_ref_yx( 0, 2 ) =            - _numc(2.0) * _c * _a;
  1566. get_item_ref_yx( 1, 2 ) =            - _numc(2.0) * _c * _b;
  1567. get_item_ref_yx( 2, 2 ) = _numc(1.0) - _numc(2.0) * _c * _c;
  1568. get_item_ref_yx( 3, 2 ) =            - _numc(2.0) * _d * _c;
  1569. get_item_ref_yx( 0, 3 ) = _numc(0.0);
  1570. get_item_ref_yx( 1, 3 ) = _numc(0.0);
  1571. get_item_ref_yx( 2, 3 ) = _numc(0.0);
  1572. get_item_ref_yx( 3, 3 ) = _numc(1.0);
  1573. }
  1574. void load_reflection( _v4rct plane )
  1575. {
  1576. load_reflection( plane.a, plane.b, plane.c, plane.d );
  1577. }
  1579. };
  1581. template < class _num, class _ntr >
  1582. inline matrix<_num,_ntr> operator * (
  1583. const matrix<_num,_ntr> &_mtx,
  1584. const _num _v
  1585. ) 
  1586. {
  1587. matrix<_num,_ntr> _ret_val;
  1588. for( int _i = 0; _i < 16; _i++ )
  1589. *(((_num*)&_ret_val.m11)+_i) = *(((_num*)&_mtx.m11)+_i) * _v;
  1590. return _ret_val;
  1591. }
  1593. template < class _num, class _ntr >
  1594. inline matrix<_num,_ntr> operator * (
  1595. const _num _v,
  1596. const matrix<_num,_ntr> & _mtx
  1597. ) 
  1598. {
  1599. matrix<_num,_ntr> _ret_val;
  1600. for( int _i = 0; _i < 16; _i++ )
  1601. *(((_num*)&_ret_val.m11)+_i) = *(((_num*)&_mtx.m11)+_i) * _v;
  1602. return _ret_val;
  1603. }
  1605. template < class _num, class _ntr >
  1606. inline matrix<_num,_ntr> operator * (
  1607. const matrix<_num,_ntr> & _mtx1,
  1608. const matrix<_num,_ntr> & _mtx2
  1609. ) 
  1610. {
  1611. return matrix<_num,_ntr>::stat_multiplication( _mtx1, _mtx2 );
  1612. }
  1614. template < class _num, class _ntr >
  1615. inline matrix<_num,_ntr> operator + (
  1616. const matrix<_num,_ntr> & _mtx1,
  1617. const matrix<_num,_ntr> & _mtx2
  1618. ) 
  1619. {
  1620. matrix<_num,_ntr> _ret_val;
  1621. for( int _i = 0; _i < 16; _i++ )
  1622. *(((_num*)&_ret_val.m11)+_i) = *(((_num*)&_mtx1.m11)+_i) + *(((_num*)&_mtx2.m11)+_i);
  1623. return _ret_val;
  1624. }
  1626. template < class _num, class _ntr >
  1627. inline matrix<_num,_ntr> operator - (
  1628. const matrix<_num,_ntr> & _mtx1,
  1629. const matrix<_num,_ntr> & _mtx2
  1630. )
  1631. {
  1632. matrix<_num,_ntr> _ret_val;
  1633. for( int _i = 0; _i < 16; _i++ )
  1634. *(((_num*)&_ret_val.m11)+_i) = *(((_num*)&_mtx1.m11)+_i) - *(((_num*)&_mtx2.m11)+_i);
  1635. return _ret_val;
  1636. }
  1638. template < class _num, class _ntr >
  1639. inline matrix<_num,_ntr> operator - ( const matrix<_num,_ntr> & _mtx ) 
  1640. {
  1641. matrix<_num,_ntr> _ret_val;
  1642. for( int _i = 0; _i < 16; _i++ )
  1643. *(((_num*)&_ret_val.m11)+_i) = - *(((_num*)&_mtx.m11)+_i);
  1644. return _ret_val;
  1645. }
  1647. template < class _num, class _ntr >
  1648. inline matrix<_num,_ntr> operator + ( const matrix<_num,_ntr> & _mtx ) 
  1649. {
  1650. return _mtx;
  1651. }
  1653. template < class _num, class _ntr >
  1654. void inline vector3<_num,_ntr>::stat_multiplication(
  1655. const matrix<_num,_ntr> & _mtx,
  1656. const vector3<_num,_ntr> & _v,
  1657. vector3<_num,_ntr> & _ret_val
  1658. )
  1659. {
  1660. vector3<_num,_ntr> _ret_val_prep;
  1661. _ret_val_prep.x = _mtx.m11*_v.x + _mtx.m12*_v.y + _mtx.m13*_v.z + _mtx.m14;
  1662. _ret_val_prep.y = _mtx.m21*_v.x + _mtx.m22*_v.y + _mtx.m23*_v.z + _mtx.m24;
  1663. _ret_val_prep.z = _mtx.m31*_v.x + _mtx.m32*_v.y + _mtx.m33*_v.z + _mtx.m34;
  1664. _ret_val.x = _ret_val_prep.x;
  1665. _ret_val.y = _ret_val_prep.y;
  1666. _ret_val.z = _ret_val_prep.z;
  1667. }
  1669. template < class _num, class _ntr >
  1670. inline vector3<_num,_ntr> vector3<_num,_ntr>::stat_multiplication(
  1671. const matrix<_num,_ntr> & _mtx,
  1672. const vector3<_num,_ntr> & _v
  1673. )
  1674. {
  1675. vector3<_num,_ntr> _ret_val;
  1676. _ret_val.x = _mtx.m11*_v.x + _mtx.m12*_v.y + _mtx.m13*_v.z + _mtx.m14;
  1677. _ret_val.y = _mtx.m21*_v.x + _mtx.m22*_v.y + _mtx.m23*_v.z + _mtx.m24;
  1678. _ret_val.z = _mtx.m31*_v.x + _mtx.m32*_v.y + _mtx.m33*_v.z + _mtx.m34;
  1679. return _ret_val;
  1680. }
  1682. template < class _num, class _ntr >
  1683. inline vector3<_num,_ntr> operator * (
  1684. const matrix<_num,_ntr> & _mtx,
  1685. const vector3<_num,_ntr> & _v
  1686. )
  1687. {
  1688. return vector3<_num,_ntr>::stat_multiplication( _mtx, _v );
  1689. }
  1691. template < class _num, class _ntr >
  1692. inline void vector4<_num,_ntr>::stat_multiplication(
  1693. const matrix<_num,_ntr> & _mtx,
  1694. _v4rct _v,
  1695. _v4rt _ret_val
  1696. )
  1697. {
  1698. vector4<_num,_ntr> _ret_val_prep;
  1699. _ret_val_prep.x = _mtx.m11*_v.x + _mtx.m12*_v.y + _mtx.m13*_v.z + _mtx.m14*_v.w;
  1700. _ret_val_prep.y = _mtx.m21*_v.x + _mtx.m22*_v.y + _mtx.m23*_v.z + _mtx.m24*_v.w;
  1701. _ret_val_prep.z = _mtx.m31*_v.x + _mtx.m32*_v.y + _mtx.m33*_v.z + _mtx.m34*_v.w;
  1702. _ret_val_prep.w = _mtx.m41*_v.x + _mtx.m42*_v.y + _mtx.m43*_v.z + _mtx.m44*_v.w;
  1703. _ret_val.x = _ret_val_prep.x;
  1704. _ret_val.y = _ret_val_prep.y;
  1705. _ret_val.z = _ret_val_prep.z;
  1706. _ret_val.w = _ret_val_prep.w;
  1707. }
  1709. template < class _num, class _ntr >
  1710. inline vector4<_num,_ntr> vector4<_num,_ntr>::stat_multiplication(
  1711. const matrix<_num,_ntr> & _mtx,
  1712. _v4rct _v
  1713. )
  1714. {
  1715. vector4<_num,_ntr> _ret_val;
  1716. _ret_val.x = _mtx.m11*_v.x + _mtx.m12*_v.y + _mtx.m13*_v.z + _mtx.m14*_v.w;
  1717. _ret_val.y = _mtx.m21*_v.x + _mtx.m22*_v.y + _mtx.m23*_v.z + _mtx.m24*_v.w;
  1718. _ret_val.z = _mtx.m31*_v.x + _mtx.m32*_v.y + _mtx.m33*_v.z + _mtx.m34*_v.w;
  1719. _ret_val.w = _mtx.m41*_v.x + _mtx.m42*_v.y + _mtx.m43*_v.z + _mtx.m44*_v.w;
  1720. return _ret_val;
  1721. }
  1723. template < class _num, class _ntr >
  1724. inline vector4<_num,_ntr> operator * (
  1725. const matrix<_num,_ntr> & _mtx,
  1726. const vector4<_num,_ntr> & _v
  1727. )
  1728. {
  1729. return vector4<_num,_ntr>::stat_multiplication( _mtx, _v );
  1730. }
  1732. template < class _num, class _ntr >
  1733. inline quaternion<_num,_ntr>::quaternion(
  1734. matrix<_num,_ntr> & _mtx
  1735. )
  1736. {
  1737. _num _tr, _s, _q[4];
  1738. int _i, _j, _k;
  1739. int _nxt_val[3] = { 1, 2, 0 };
  1740. _tr = _mtx.m11 + _mtx.m22 + _mtx.m33;
  1741. if( _tr > ((_num)0.0) ) 
  1742. {
  1743. _s = _ntr::sqrt( _tr + ((_num)1.0) );
  1744. w  = _s / ((_num)2.0);
  1745. _s = ((_num)0.5) / _s;
  1746. x  = (_mtx.m23 - _mtx.m32) * _s;
  1747. y  = (_mtx.m31 - _mtx.m13) * _s;
  1748. z  = (_mtx.m12 - _mtx.m21) * _s;
  1749. } 
  1750. else 
  1751. {
  1752.      _i = 0;
  1753. if( _mtx.m22 > _mtx.m11 )
  1754. _i = 1;
  1755. if( _mtx.m33 > _mtx.arr[4*_i+_i] )
  1756. _i = 2;
  1757. _j = _nxt_val[_i];
  1758. _k = _nxt_val[_j];
  1759. _s =
  1760. _ntr::sqrt(
  1761. ( _mtx.arr[4*_i+_i] -
  1762. ( _mtx.arr[4*_j+_j] + _mtx.arr[4*_k+_k] )
  1763. ) + ((_num)1.0)
  1764. );
  1765. _q[_i] = _s * ((_num)0.5);
  1766. if( _s != ((_num)0.0) )
  1767. _s = ((_num)0.5) / _s;
  1768. _q[3]  = (_mtx.arr[4*_j+_k] - _mtx.arr[4*_k+_j] ) * _s;
  1769. _q[_j] = (_mtx.arr[4*_i+_j] + _mtx.arr[4*_j+_i] ) * _s;
  1770. _q[_k] = (_mtx.arr[4*_i+_k] + _mtx.arr[4*_k+_i] ) * _s;
  1771. x = _q[0];
  1772. y = _q[1];
  1773. z = _q[2];
  1774. w = _q[3];
  1775. }
  1776. }
  1778. template < class _num, class _ntr >
  1779. inline void quaternion<_num,_ntr>::get_as_matrix(
  1780. matrix<_num,_ntr> & _mtx
  1781. )
  1782. {
  1783. normalize();
  1784. _num xx = x * x;
  1785. _num yy = y * y;
  1786. _num zz = z * z;
  1787. _mtx.get_item_ref_yx( 0, 0 ) = ((_num)1.0) - ((_num)2.0) * ( yy + zz );
  1788. _mtx.get_item_ref_yx( 1, 0 ) = ((_num)2.0) * ( x * y + w * z );
  1789. _mtx.get_item_ref_yx( 2, 0 ) = ((_num)2.0) * ( x * z - w * y );
  1790. _mtx.get_item_ref_yx( 3, 0 ) = ((_num)0.0);
  1791. _mtx.get_item_ref_yx( 0, 1 ) = ((_num)2.0) * ( x * y - w * z );
  1792. _mtx.get_item_ref_yx( 1, 1 ) = ((_num)1.0) - ((_num)2.0) * ( xx + zz );
  1793. _mtx.get_item_ref_yx( 2, 1 ) = ((_num)2.0) * ( y * z + w * x );
  1794. _mtx.get_item_ref_yx( 3, 1 ) = ((_num)0.0);
  1795. _mtx.get_item_ref_yx( 0, 2 ) = ((_num)2.0) * ( x * z + w * y );
  1796. _mtx.get_item_ref_yx( 1, 2 ) = ((_num)2.0) * ( y * z - w * x );
  1797. _mtx.get_item_ref_yx( 2, 2 ) = ((_num)1.0) - ((_num)2.0) * ( xx + yy );
  1798. _mtx.get_item_ref_yx( 3, 2 ) = ((_num)0.0);
  1799. _mtx.get_item_ref_yx( 0, 3 ) = ((_num)0.0);
  1800. _mtx.get_item_ref_yx( 1, 3 ) = ((_num)0.0);
  1801. _mtx.get_item_ref_yx( 2, 3 ) = ((_num)0.0);
  1802. _mtx.get_item_ref_yx( 3, 3 ) = ((_num)1.0);
  1803. }
  1805. template < class _num, class _ntr >
  1806. inline matrix<_num,_ntr> quaternion<_num,_ntr>::
  1807. get_as_matrix() const
  1808. {
  1809. _q4t _q = (*this);
  1810. _mt _mtx;
  1811. _q.get_as_matrix( _mtx );
  1812. return _mtx;
  1813. }
  1815. template < class _num, class _ntr >
  1816. inline matrix<_num,_ntr> &
  1817. matrix<_num,_ntr>::operator *= (
  1818. const quaternion<_num,_ntr> & _q
  1819. )
  1820. {
  1821. (*this) *= _q.get_as_matrix();
  1822. return (*this);
  1823. }
  1825. template < class _num, class _ntr >
  1826. inline matrix<_num,_ntr> operator * (
  1827. const matrix<_num,_ntr> & _mtx,
  1828. const quaternion<_num,_ntr> & _q
  1829. )
  1830. {
  1831. matrix<_num,_ntr> _ret_val = _mtx;
  1832. _ret_val *= _q;
  1833. return _ret_val;
  1834. }
  1836. template < class _num, class _ntr >
  1837. inline matrix<_num,_ntr>
  1838. vector3<_num,_ntr>::get_as_scale() const
  1839. {
  1840. _mt _mtx;
  1841. _mtx.load_scale( *this );
  1842. return _mtx;
  1843. }
  1845. template < class _num, class _ntr >
  1846. inline matrix<_num,_ntr>
  1847. vector3<_num,_ntr>::get_as_translation() const
  1848. {
  1849. _mt _mtx;
  1850. _mtx.load_translation( *this );
  1851. return _mtx;
  1852. }
  1854. template < class _num, class _ntr >
  1855. inline matrix<_num,_ntr>
  1856. vector3<_num,_ntr>::get_as_rotation_xyz() const
  1857. {
  1858. _mt _mtx;
  1859. _mtx.load_rotation_xyz( *this );
  1860. return _mtx;
  1861. }
  1863. template < class _num, class _ntr >
  1864. inline matrix<_num,_ntr>
  1865. vector3<_num,_ntr>::get_as_rotation_quaternion() const
  1866. {
  1867. quaternion<_num,_ntr> _q( *this );
  1868. return _q.get_as_matrix();
  1869. }
  1871. }; // namespace gltl
  1873. // rollback warning C4201
  1874. #pragma warning( pop )
  1876. #endif // __GLTL_H