You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

5938 lines
172 KiB

5 years ago
  1. #pragma once
  2. #include <cstdio>
  3. #include <cassert>
  4. #include <cmath>
  5. #include <stdlib.h>
  6. #include <vector>
  7. #include <map>
  8. #include <limits>
  9. namespace CELL
  10. {
  11. #define PI 3.14159265358979323
  12. #define TWO_PI 6.28318530717958647
  13. #define HALF_PI 1.57079632679489661
  14. #define DEG2RAD(theta) (0.01745329251994329 * (theta))
  15. #define RAD2DEG 57.2957795130823208
  16. #define LOG2 0.69314718055994529
  17. #define WGS_84_RADIUS_EQUATOR 6378137.0
  18. #define WGS_84_RADIUS_POLAR 6356752.3142
  19. #define MIN(a,b) ((a) < (b) ? (a) : (b))
  20. #define MAX(a,b) ((a) > (b) ? (a) : (b))
  21. #ifndef FLT_MAX
  22. #define FLT_MAX 3.402823466e+38F
  23. #endif
  24. #ifndef FLT_MIN
  25. #define FLT_MIN 1.175494351e-38F
  26. #endif
  27. #define MAKE_INT(a, b) ((int)(((short)(((int)(a)) & 0xffff)) | ((int)((short)(((int)(b)) & 0xffff))) << 16))
  28. typedef unsigned char byte;
  29. typedef long long int64;
  30. typedef unsigned short ushort;
  31. typedef unsigned int uint;
  32. typedef unsigned long ulong;
  33. template<class T> inline T tmin(T a,T b)
  34. {
  35. return a < b ? a:b;
  36. }
  37. template<class T> inline T tmax(T a,T b)
  38. {
  39. return a > b ? a:b;
  40. }
  41. union LargeInt
  42. {
  43. struct __LARGE_INT
  44. {
  45. unsigned int LowPart;
  46. unsigned int HighPart;
  47. }_largeInt;
  48. int64 int64Data;
  49. } ;
  50. inline float unitRandom ()
  51. {
  52. return float(rand()) / float( RAND_MAX );
  53. }
  54. //-----------------------------------------------------------------------
  55. inline float rangeRandom (float fLow, float fHigh)
  56. {
  57. return (fHigh-fLow)*unitRandom() + fLow;
  58. }
  59. /**
  60. * 64λ
  61. */
  62. inline int64 makeInt64(unsigned low,unsigned hi)
  63. {
  64. LargeInt intdata;
  65. intdata._largeInt.HighPart = low;
  66. intdata._largeInt.LowPart = hi;
  67. return intdata.int64Data;
  68. }
  69. template <typename T>
  70. struct tvec2
  71. {
  72. typedef T value_type;
  73. typedef std::size_t size_type;
  74. typedef tvec2<T> type;
  75. value_type x;
  76. value_type y;
  77. value_type & operator[](size_type i)
  78. {
  79. assert(i < this->length());
  80. return (&x)[i];
  81. }
  82. value_type const & operator[]( size_type i ) const
  83. {
  84. assert(i < this->length());
  85. return (&x)[i];
  86. }
  87. tvec2() :
  88. x(value_type(0)),
  89. y(value_type(0))
  90. {}
  91. tvec2(tvec2<T> const & v) :
  92. x(v.x),
  93. y(v.y)
  94. {}
  95. tvec2(value_type const & s) :
  96. x(s),
  97. y(s)
  98. {}
  99. tvec2(value_type const & s1, value_type const & s2) :
  100. x(s1),
  101. y(s2)
  102. {}
  103. template <typename U>
  104. tvec2(U const & x) :
  105. x(value_type(x)),
  106. y(value_type(x))
  107. {}
  108. template <typename U, typename V>
  109. tvec2(U const & a, V b) :
  110. x(value_type(a)),
  111. y(value_type(b))
  112. {}
  113. template <typename U>
  114. tvec2(tvec2<U> const & v) :
  115. x(value_type(v.x)),
  116. y(value_type(v.y))
  117. {}
  118. tvec2<T> & operator= (tvec2<T> const & v)
  119. {
  120. this->x = v.x;
  121. this->y = v.y;
  122. return *this;
  123. }
  124. template <typename U>
  125. tvec2<T> & operator= (tvec2<U> const & v)
  126. {
  127. this->x = T(v.x);
  128. this->y = T(v.y);
  129. return *this;
  130. }
  131. template <typename U>
  132. tvec2<T> & operator+=(U const & s)
  133. {
  134. this->x += T(s);
  135. this->y += T(s);
  136. return *this;
  137. }
  138. template <typename U>
  139. tvec2<T> & operator+=(tvec2<U> const & v)
  140. {
  141. this->x += T(v.x);
  142. this->y += T(v.y);
  143. return *this;
  144. }
  145. template <typename U>
  146. tvec2<T> & operator-=(U const & s)
  147. {
  148. this->x -= T(s);
  149. this->y -= T(s);
  150. return *this;
  151. }
  152. template <typename U>
  153. tvec2<T> & operator-=(tvec2<U> const & v)
  154. {
  155. this->x -= T(v.x);
  156. this->y -= T(v.y);
  157. return *this;
  158. }
  159. template <typename U>
  160. tvec2<T> & operator*=(U s)
  161. {
  162. this->x *= T(s);
  163. this->y *= T(s);
  164. return *this;
  165. }
  166. template <typename U>
  167. tvec2<T> & operator*=(tvec2<U> const & v)
  168. {
  169. this->x *= T(v.x);
  170. this->y *= T(v.y);
  171. return *this;
  172. }
  173. template <typename U>
  174. tvec2<T> & operator/=(U s)
  175. {
  176. this->x /= T(s);
  177. this->y /= T(s);
  178. return *this;
  179. }
  180. template <typename U>
  181. tvec2<T> & operator/=(tvec2<U> const & v)
  182. {
  183. this->x /= T(v.x);
  184. this->y /= T(v.y);
  185. return *this;
  186. }
  187. tvec2<T> & operator++()
  188. {
  189. ++ this->x;
  190. ++ this->y;
  191. return *this;
  192. }
  193. tvec2<T> & operator--()
  194. {
  195. --this->x;
  196. --this->y;
  197. return *this;
  198. }
  199. void makeCeil( tvec2<T> cmp )
  200. {
  201. if( cmp.x > x ) x = cmp.x;
  202. if( cmp.y > y ) y = cmp.y;
  203. }
  204. void makeFloor( tvec2<T> cmp )
  205. {
  206. if( cmp.x < x ) x = cmp.x;
  207. if( cmp.y < y ) y = cmp.y;
  208. }
  209. };
  210. template <typename T>
  211. tvec2<T> rotate(tvec2<T> const & v, T angle)
  212. {
  213. tvec2<T> res;
  214. T const c(cos(DEG2RAD(angle)));
  215. T const s(sin(DEG2RAD(angle)));
  216. res.x = v.x * c - v.y * s;
  217. res.y = v.x * s + v.y * c;
  218. return res;
  219. }
  220. template <typename T>
  221. bool operator==(tvec2<T> const & v1, tvec2<T> const & v2)
  222. {
  223. return (v1.x == v2.x) && (v1.y == v2.y);
  224. }
  225. template <typename T>
  226. bool operator!=(tvec2<T> const & v1, tvec2<T> const & v2)
  227. {
  228. return (v1.x != v2.x) || (v1.y != v2.y);
  229. }
  230. template <typename T>
  231. tvec2<T> operator+ (tvec2<T> const & v, T const & s)
  232. {
  233. return tvec2<T>(
  234. v.x + T(s),
  235. v.y + T(s));
  236. }
  237. template <typename T>
  238. tvec2<T> operator+ (T const & s, tvec2<T> const & v)
  239. {
  240. return tvec2<T>(
  241. T(s) + v.x,
  242. T(s) + v.y);
  243. }
  244. template <typename T>
  245. tvec2<T> operator+ (tvec2<T> const & v1, tvec2<T> const & v2)
  246. {
  247. return tvec2<T>(
  248. v1.x + T(v2.x),
  249. v1.y + T(v2.y));
  250. }
  251. template <typename T>
  252. tvec2<T> operator-(tvec2<T> const & v, T const & s)
  253. {
  254. return tvec2<T>(
  255. v.x - T(s),
  256. v.y - T(s));
  257. }
  258. template <typename T>
  259. tvec2<T> operator- (T const & s, tvec2<T> const & v)
  260. {
  261. return tvec2<T>(
  262. T(s) - v.x,
  263. T(s) - v.y);
  264. }
  265. template <typename T>
  266. tvec2<T> operator- (tvec2<T> const & v1, tvec2<T> const & v2)
  267. {
  268. return tvec2<T>(
  269. v1.x - T(v2.x),
  270. v1.y - T(v2.y));
  271. }
  272. template <typename T>
  273. tvec2<T> operator* (tvec2<T> const & v, T const & s)
  274. {
  275. return tvec2<T>(
  276. v.x * T(s),
  277. v.y * T(s));
  278. }
  279. template <typename T>
  280. tvec2<T> operator* (T const & s, tvec2<T> const & v)
  281. {
  282. return tvec2<T>(
  283. T(s) * v.x,
  284. T(s) * v.y);
  285. }
  286. template <typename T>
  287. tvec2<T> operator*(tvec2<T> const & v1, tvec2<T> const & v2)
  288. {
  289. return tvec2<T>(
  290. v1.x * T(v2.x),
  291. v1.y * T(v2.y));
  292. }
  293. template <typename T>
  294. tvec2<T> operator/(tvec2<T> const & v, T const & s)
  295. {
  296. return tvec2<T>(
  297. v.x / T(s),
  298. v.y / T(s));
  299. }
  300. template <typename T>
  301. tvec2<T> operator/(T const & s, tvec2<T> const & v)
  302. {
  303. return tvec2<T>(
  304. T(s) / v.x,
  305. T(s) / v.y);
  306. }
  307. template <typename T>
  308. tvec2<T> operator/ (tvec2<T> const & v1, tvec2<T> const & v2 )
  309. {
  310. return tvec2<T>(
  311. v1.x / T(v2.x),
  312. v1.y / T(v2.y)
  313. );
  314. }
  315. template <typename T>
  316. tvec2<T> operator- (tvec2<T> const & v)
  317. {
  318. return tvec2<T> (
  319. -v.x,
  320. -v.y
  321. );
  322. }
  323. template <typename T>
  324. tvec2<T> operator++ (tvec2<T> const & v, int)
  325. {
  326. return tvec2<T>(
  327. v.x + T(1),
  328. v.y + T(1)
  329. );
  330. }
  331. template <typename T>
  332. tvec2<T> operator-- (tvec2<T> const & v, int)
  333. {
  334. return tvec2<T>(
  335. v.x - T(1),
  336. v.y - T(1)
  337. );
  338. }
  339. template <typename T>
  340. struct tvec3
  341. {
  342. typedef T value_type;
  343. typedef std::size_t size_type;
  344. typedef tvec3<T> type;
  345. value_type x;
  346. value_type y;
  347. value_type z;
  348. size_type length() const
  349. {
  350. return 3;
  351. }
  352. value_type & operator[](size_type i)
  353. {
  354. assert(i < this->length());
  355. return (&x)[i];
  356. }
  357. value_type const & operator[](size_type i) const
  358. {
  359. assert(i < this->length());
  360. return (&x)[i];
  361. }
  362. inline tvec3() :
  363. x(value_type(0)),
  364. y(value_type(0)),
  365. z(value_type(0))
  366. {}
  367. inline tvec3(tvec3<T> const & v) :
  368. x(v.x),
  369. y(v.y),
  370. z(v.z)
  371. {}
  372. inline tvec3(value_type s) :
  373. x(s),
  374. y(s),
  375. z(s)
  376. {}
  377. inline tvec3(value_type s0, value_type s1, value_type s2) :
  378. x(s0),
  379. y(s1),
  380. z(s2)
  381. {}
  382. template <typename U>
  383. tvec3(U s) :
  384. x(value_type(s)),
  385. y(value_type(s)),
  386. z(value_type(s))
  387. {}
  388. template <typename A, typename B, typename C>
  389. tvec3(A x, B y, C z) :
  390. x(value_type(x)),
  391. y(value_type(y)),
  392. z(value_type(z))
  393. {}
  394. template <typename A, typename B>
  395. tvec3(tvec2<A> const& v, B s) :
  396. x(value_type(v.x)),
  397. y(value_type(v.y)),
  398. z(value_type(s))
  399. {}
  400. template <typename A, typename B>
  401. tvec3(A s,tvec2<B> const& v) :
  402. x(value_type(s)),
  403. y(value_type(v.x)),
  404. z(value_type(v.y))
  405. {}
  406. template <typename U>
  407. tvec3(tvec3<U> const & v) :
  408. x(value_type(v.x)),
  409. y(value_type(v.y)),
  410. z(value_type(v.z))
  411. {}
  412. tvec3<T>& operator= (tvec3<T> const & v)
  413. {
  414. this->x = v.x;
  415. this->y = v.y;
  416. this->z = v.z;
  417. return *this;
  418. }
  419. template <typename U>
  420. tvec3<T>& operator= (tvec3<U> const & v)
  421. {
  422. this->x = T(v.x);
  423. this->y = T(v.y);
  424. this->z = T(v.z);
  425. return *this;
  426. }
  427. template <typename U>
  428. tvec3<T> & operator+=(U const & s)
  429. {
  430. this->x += T(s);
  431. this->y += T(s);
  432. this->z += T(s);
  433. return *this;
  434. }
  435. template <typename U>
  436. tvec3<T> & operator+=(tvec3<U> const & v)
  437. {
  438. this->x += T(v.x);
  439. this->y += T(v.y);
  440. this->z += T(v.z);
  441. return *this;
  442. }
  443. template <typename U>
  444. tvec3<T> & operator-=(U const & s)
  445. {
  446. this->x -= T(s);
  447. this->y -= T(s);
  448. this->z -= T(s);
  449. return *this;
  450. }
  451. template <typename U>
  452. tvec3<T> & operator-=(tvec3<U> const & v)
  453. {
  454. this->x -= T(v.x);
  455. this->y -= T(v.y);
  456. this->z -= T(v.z);
  457. return *this;
  458. }
  459. template <typename U>
  460. tvec3<T> & operator*=(U const & s)
  461. {
  462. this->x *= T(s);
  463. this->y *= T(s);
  464. this->z *= T(s);
  465. return *this;
  466. }
  467. template <typename U>
  468. tvec3<T> & operator*=(tvec3<U> const & v)
  469. {
  470. this->x *= T(v.x);
  471. this->y *= T(v.y);
  472. this->z *= T(v.z);
  473. return *this;
  474. }
  475. template <typename U>
  476. tvec3<T> & operator/=(U const & s)
  477. {
  478. this->x /= T(s);
  479. this->y /= T(s);
  480. this->z /= T(s);
  481. return *this;
  482. }
  483. template <typename U>
  484. tvec3<T> & operator/=(tvec3<U> const & v)
  485. {
  486. this->x /= T(v.x);
  487. this->y /= T(v.y);
  488. this->z /= T(v.z);
  489. return *this;
  490. }
  491. tvec3<T> & operator++()
  492. {
  493. ++this->x;
  494. ++this->y;
  495. ++this->z;
  496. return *this;
  497. }
  498. tvec3<T> & operator--()
  499. {
  500. --this->x;
  501. --this->y;
  502. --this->z;
  503. return *this;
  504. }
  505. void makeFloor( const tvec3<T>& cmp )
  506. {
  507. if( cmp.x < x ) x = cmp.x;
  508. if( cmp.y < y ) y = cmp.y;
  509. if( cmp.z < z ) z = cmp.z;
  510. }
  511. void makeCeil( const tvec3<T>& cmp )
  512. {
  513. if( cmp.x > x ) x = cmp.x;
  514. if( cmp.y > y ) y = cmp.y;
  515. if( cmp.z > z ) z = cmp.z;
  516. }
  517. T lengthf() const
  518. {
  519. return (T)sqrtf( x * x + y * y + z * z );
  520. }
  521. };
  522. template<typename T>
  523. bool operator >(const tvec3<T>& left ,const tvec3<T>& right)
  524. {
  525. return left.x > right.x && left.y > right.y && left.z > right.z;
  526. }
  527. template<typename T>
  528. bool operator <(const tvec3<T>& left ,const tvec3<T>& right)
  529. {
  530. return left.x < right.x && left.y < right.y && left.z < right.z;
  531. }
  532. template <typename T>
  533. tvec3<T> rotateX(const tvec3<T>& v, T angle)
  534. {
  535. tvec3<T> res(v);
  536. T c = cos(T(DEG2RAD(angle)));
  537. T s = sin(T(DEG2RAD(angle)));
  538. res.y = v.y * c - v.z * s;
  539. res.z = v.y * s + v.z * c;
  540. return res;
  541. }
  542. template <typename T>
  543. tvec3<T> rotateY(tvec3<T> const & v, T angle)
  544. {
  545. tvec3<T> res = v;
  546. T c = cos(T(DEG2RAD(angle)));
  547. T s = sin(T(DEG2RAD(angle)));
  548. res.x = v.x * c + v.z * s;
  549. res.z = -v.x * s + v.z * c;
  550. return res;
  551. }
  552. template <typename T>
  553. tvec3<T> rotateZ(tvec3<T> const & v, T angle)
  554. {
  555. tvec3<T> res = v;
  556. T c = cos(DEG2RAD(angle));
  557. T s = sin(DEG2RAD(angle));
  558. res.x = v.x * c - v.y * s;
  559. res.y = v.x * s + v.y * c;
  560. return res;
  561. }
  562. /**
  563. * ļн
  564. * A,B
  565. * AB = |A|*|B|*cos(@)
  566. * cos(@) = AB/|A|*|B|
  567. * @ = acos(@)
  568. */
  569. template <typename T>
  570. T angleBetweenVector(const tvec3<T>& a, const tvec3<T>& b)
  571. {
  572. #define Mag(V) (sqrtf(V.x*V.x + V.y*V.y + V.z*V.z))
  573. T dotProduct = dot(a, b);
  574. T vectorsMagnitude = Mag(a) * Mag(b) ;
  575. T angle = acos( dotProduct / vectorsMagnitude );
  576. T result = angle * T(RAD2DEG);
  577. if(_isnan(result))
  578. {
  579. return T(0);
  580. }
  581. else
  582. {
  583. return result;
  584. }
  585. }
  586. template<typename T>
  587. inline bool _isnan(T t)
  588. {
  589. return t == t;
  590. }
  591. template <typename T>
  592. T angleBetweenVector(const tvec2<T>& a, const tvec2<T>& b)
  593. {
  594. #define Mag2D(V) (sqrtf(V.x*V.x + V.y*V.y))
  595. T dotProduct = dot(a, b);
  596. T vectorsMagnitude = Mag2D(a) * Mag2D(b) ;
  597. T angle = acos( dotProduct / vectorsMagnitude );
  598. T result = angle * T(RAD2DEG);
  599. if(_isnan(result))
  600. {
  601. return T(0);
  602. }
  603. else
  604. {
  605. return result;
  606. }
  607. }
  608. template <typename T>
  609. static T clamp(T val, T minval, T maxval)
  610. {
  611. assert (minval < maxval && "Invalid clamp range");
  612. return MAX(MIN(val, maxval), minval);
  613. }
  614. template <typename T>
  615. inline T acosEx (T val)
  616. {
  617. if ( T(-1.0f) < val )
  618. {
  619. if ( val < 1.0f )
  620. return T(acos(val));
  621. else
  622. return T(0);
  623. }
  624. else
  625. {
  626. return T(PI);
  627. }
  628. }
  629. template<typename T>
  630. inline T angleBetween(const tvec3<T>& a, const tvec3<T>& b)
  631. {
  632. T lenProduct = a.lengthf() * b.lengthf();
  633. // Divide by zero check
  634. if(lenProduct < 1e-6f)
  635. lenProduct = 1e-6f;
  636. float f = dot(a,b) / lenProduct;
  637. f = clamp(f, T(-1.0), T(1.0));
  638. return acosEx(f);
  639. }
  640. /**
  641. * ڶ
  642. * ڶУ򣬵ߵļн֮ == 360
  643. */
  644. template<typename T>
  645. bool insidePolyon( const tvec3<T>& point, const tvec3<T> polygon[], size_t count)
  646. {
  647. tvec3<T> vA, vB;
  648. T angle = T(0.0);
  649. for (size_t i = 0; i < count; ++i)
  650. {
  651. vA = polygon[i] - point;
  652. vB = polygon[(i + 1) % count] - point;
  653. angle += angleBetweenVector(vA, vB);
  654. }
  655. if( abs(angle - 360 ) >= 0.5f)
  656. {
  657. return true;
  658. }
  659. return false;
  660. }
  661. template<typename T>
  662. bool insidePolyon( const tvec2<T>& point, const tvec2<T> polygon[], size_t count)
  663. {
  664. T angle = T(0.0);
  665. tvec2<T> vA, vB;
  666. for (size_t i = 0; i < count; ++i)
  667. {
  668. vA = polygon[i] - point;
  669. vB = polygon[(i + 1) % count] - point;
  670. tvec3<T> a(vA.x,vA.y,0);
  671. tvec3<T> b(vB.x,vB.y,0);
  672. angle += angleBetweenVector<T>(a, b);
  673. }
  674. if( abs(angle - 360 ) >= 0.5f)
  675. {
  676. return true;
  677. }
  678. return false;
  679. }
  680. template<typename T>
  681. bool pointinTriangle(tvec3<T> A, tvec3<T> B, tvec3<T> C, tvec3<T> P)
  682. {
  683. tvec3<T> v0 = C - A ;
  684. tvec3<T> v1 = B - A ;
  685. tvec3<T> v2 = P - A ;
  686. float dot00 = dot(v0,v0) ;
  687. float dot01 = dot(v0,v1) ;
  688. float dot02 = dot(v0,v2) ;
  689. float dot11 = dot(v1,v1) ;
  690. float dot12 = dot(v1,v2) ;
  691. float inverDeno = 1 / (dot00 * dot11 - dot01 * dot01) ;
  692. float u = (dot11 * dot02 - dot01 * dot12) * inverDeno ;
  693. if (u < 0 || u > 1) // if u out of range, return directly
  694. {
  695. return false ;
  696. }
  697. float v = (dot00 * dot12 - dot01 * dot02) * inverDeno ;
  698. if (v < 0 || v > 1) // if v out of range, return directly
  699. {
  700. return false ;
  701. }
  702. return u + v <= 1 ;
  703. }
  704. template<typename T>
  705. bool pointinTriangle(tvec2<T> A, tvec2<T> B, tvec2<T> C, tvec2<T> P)
  706. {
  707. return pointinTriangle(
  708. tvec3<T>(A.x,A.y,0),
  709. tvec3<T>(B.x,B.y,0),
  710. tvec3<T>(C.x,C.y,0),
  711. tvec3<T>(P.x,P.y,0));
  712. }
  713. /**
  714. *
  715. */
  716. template<typename T>
  717. bool intersectTriangle(
  718. const tvec3<T>& orig,
  719. const tvec3<T>& dir,
  720. tvec3<T>& v0,
  721. tvec3<T>& v1,
  722. tvec3<T>& v2,
  723. T* t,
  724. T* u,
  725. T* v
  726. )
  727. {
  728. // Find vectors for two edges sharing vert0
  729. tvec3<T> edge1 = v1 - v0;
  730. tvec3<T> edge2 = v2 - v0;
  731. // Begin calculating determinant - also used to calculate U parameter
  732. tvec3<T> pvec;
  733. pvec = cross(dir, edge2 );
  734. // If determinant is near zero, ray lies in plane of triangle
  735. T det = dot( edge1,pvec );
  736. tvec3<T> tvec;
  737. if( det > 0 )
  738. {
  739. tvec = orig - v0;
  740. }
  741. else
  742. {
  743. tvec = v0 - orig;
  744. det = -det;
  745. }
  746. if( det < 0.0001f )
  747. return false;
  748. // Calculate U parameter and test bounds
  749. *u = dot( tvec, pvec );
  750. if( *u < 0.0f || *u > det )
  751. return false;
  752. // Prepare to test V parameter
  753. tvec3<T> qvec;
  754. qvec = cross(tvec, edge1 );
  755. // Calculate V parameter and test bounds
  756. *v = dot( dir, qvec );
  757. if( *v < T(0.0f) || *u + *v > det )
  758. return false;
  759. *t = dot( edge2,qvec );
  760. T fInvDet = T(1.0) / det;
  761. *t *= fInvDet;
  762. *u *= fInvDet;
  763. *v *= fInvDet;
  764. return true;
  765. }
  766. /**
  767. *
  768. */
  769. template<typename T> T calcTriangleArea(const tvec3<T>& pt1,const tvec3<T>& pt2,const tvec3<T>& pt3)
  770. {
  771. tvec3<T> e1 = pt2 - pt1;
  772. tvec3<T> e2 = pt3 - pt1;
  773. tvec3<T> e3 = cross(e1,e2);
  774. return length(e3) * T(0.5);
  775. }
  776. template <typename T>
  777. bool operator==(tvec3<T> const & v1, tvec3<T> const & v2)
  778. {
  779. return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z);
  780. }
  781. template <typename T>
  782. bool operator!=(tvec3<T> const & v1, tvec3<T> const & v2)
  783. {
  784. return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z);
  785. }
  786. template <typename T>
  787. tvec3<T> operator+(tvec3<T> const & v, T const & s)
  788. {
  789. return tvec3<T>(
  790. v.x + T(s),
  791. v.y + T(s),
  792. v.z + T(s));
  793. }
  794. template <typename T>
  795. tvec3<T> operator+ ( T const & s, tvec3<T> const & v)
  796. {
  797. return tvec3<T>(
  798. T(s) + v.x,
  799. T(s) + v.y,
  800. T(s) + v.z);
  801. }
  802. template <typename T>
  803. tvec3<T> operator+ (tvec3<T> const & v1, tvec3<T> const & v2)
  804. {
  805. return tvec3<T>(
  806. v1.x + T(v2.x),
  807. v1.y + T(v2.y),
  808. v1.z + T(v2.z));
  809. }
  810. template <typename T>
  811. tvec3<T> operator- (tvec3<T> const & v, T const & s)
  812. {
  813. return tvec3<T>(
  814. v.x - T(s),
  815. v.y - T(s),
  816. v.z - T(s));
  817. }
  818. template <typename T>
  819. tvec3<T> operator- (T const & s, tvec3<T> const & v)
  820. {
  821. return tvec3<T>(
  822. T(s) - v.x,
  823. T(s) - v.y,
  824. T(s) - v.z);
  825. }
  826. template <typename T>
  827. tvec3<T> operator- (tvec3<T> const & v1, tvec3<T> const & v2)
  828. {
  829. return tvec3<T>(
  830. v1.x - T(v2.x),
  831. v1.y - T(v2.y),
  832. v1.z - T(v2.z));
  833. }
  834. template <typename T>
  835. tvec3<T> operator*(tvec3<T> const & v, T const & s)
  836. {
  837. return tvec3<T>(
  838. v.x * T(s),
  839. v.y * T(s),
  840. v.z * T(s));
  841. }
  842. template <typename T>
  843. tvec3<T> operator* (T const & s, tvec3<T> const & v)
  844. {
  845. return tvec3<T>(
  846. T(s) * v.x,
  847. T(s) * v.y,
  848. T(s) * v.z);
  849. }
  850. template <typename T>
  851. tvec3<T> operator* (tvec3<T> const & v1, tvec3<T> const & v2)
  852. {
  853. return tvec3<T>(
  854. v1.x * T(v2.x),
  855. v1.y * T(v2.y),
  856. v1.z * T(v2.z));
  857. }
  858. template <typename T>
  859. tvec3<T> operator/ (tvec3<T> const & v, T const & s)
  860. {
  861. return tvec3<T>(
  862. v.x / T(s),
  863. v.y / T(s),
  864. v.z / T(s));
  865. }
  866. template <typename T>
  867. tvec3<T> operator/ (T const & s, tvec3<T> const & v)
  868. {
  869. return tvec3<T>(
  870. T(s) / v.x,
  871. T(s) / v.y,
  872. T(s) / v.z);
  873. }
  874. template <typename T>
  875. tvec3<T> operator/ (tvec3<T> const & v1, tvec3<T> const & v2)
  876. {
  877. return tvec3<T>(
  878. v1.x / T(v2.x),
  879. v1.y / T(v2.y),
  880. v1.z / T(v2.z));
  881. }
  882. template <typename T>
  883. tvec3<T> operator- (tvec3<T> const & v)
  884. {
  885. return tvec3<T>(
  886. -v.x,
  887. -v.y,
  888. -v.z);
  889. }
  890. template <typename T>
  891. tvec3<T> operator++ (tvec3<T> const & v, int)
  892. {
  893. return tvec3<T>(
  894. v.x + T(1),
  895. v.y + T(1),
  896. v.z + T(1));
  897. }
  898. template <typename T>
  899. tvec3<T> operator-- (tvec3<T> const & v, int)
  900. {
  901. return tvec3<T>(
  902. v.x - T(1),
  903. v.y - T(1),
  904. v.z - T(1));
  905. }
  906. template <typename T>
  907. struct tvec4
  908. {
  909. typedef T value_type;
  910. typedef std::size_t size_type;
  911. typedef tvec4<T> type;
  912. value_type x, y, z, w;
  913. size_type length() const
  914. {
  915. return 4;
  916. }
  917. value_type & operator[](size_type i)
  918. {
  919. assert(i < this->length());
  920. return (&x)[i];
  921. }
  922. value_type const & operator[](size_type i) const
  923. {
  924. assert(i < this->length());
  925. return (&x)[i];
  926. }
  927. tvec4() :
  928. x(value_type(0)),
  929. y(value_type(0)),
  930. z(value_type(0)),
  931. w(value_type(0))
  932. {}
  933. tvec4(tvec3<T> const& v, T s) :
  934. x(v.x),
  935. y(v.y),
  936. z(v.z),
  937. w(s)
  938. {}
  939. tvec4(T s) :
  940. x(s),
  941. y(s),
  942. z(s),
  943. w(s)
  944. {}
  945. tvec4(tvec4<T> const & v) :
  946. x(v.x),
  947. y(v.y),
  948. z(v.z),
  949. w(v.w)
  950. {}
  951. template <typename A, typename B>
  952. tvec4(tvec3<A> const & v, B s):
  953. x(value_type(v.x)),
  954. y(value_type(v.y)),
  955. z(value_type(v.z)),
  956. w(value_type(s))
  957. {}
  958. template <typename A, typename B>
  959. tvec4(A s,tvec3<B> const & v):
  960. x(value_type(s)),
  961. y(value_type(v.x)),
  962. z(value_type(v.y)),
  963. w(value_type(v.z))
  964. {}
  965. template<typename U>
  966. tvec4(tvec4<U> const & v) :
  967. x(value_type(v.x)),
  968. y(value_type(v.y)),
  969. z(value_type(v.z)),
  970. w(value_type(v.w))
  971. {}
  972. tvec4
  973. (
  974. value_type s1,
  975. value_type s2,
  976. value_type s3,
  977. value_type s4
  978. ) :
  979. x(s1),
  980. y(s2),
  981. z(s3),
  982. w(s4)
  983. {}
  984. tvec4<T> & operator=(tvec4<T> const & v)
  985. {
  986. this->x = v.x;
  987. this->y = v.y;
  988. this->z = v.z;
  989. this->w = v.w;
  990. return *this;
  991. }
  992. template <typename U>
  993. tvec4<T> & operator= (tvec4<U> const & v)
  994. {
  995. this->x = T(v.x);
  996. this->y = T(v.y);
  997. this->z = T(v.z);
  998. this->w = T(v.w);
  999. return *this;
  1000. }
  1001. template <typename U>
  1002. tvec4<T> & operator+=(U const & s)
  1003. {
  1004. this->x += T(s);
  1005. this->y += T(s);
  1006. this->z += T(s);
  1007. this->w += T(s);
  1008. return *this;
  1009. }
  1010. template <typename U>
  1011. tvec4<T> & operator+=(tvec4<U> const & v)
  1012. {
  1013. this->x += T(v.x);
  1014. this->y += T(v.y);
  1015. this->z += T(v.z);
  1016. this->w += T(v.w);
  1017. return *this;
  1018. }
  1019. template <typename U>
  1020. tvec4<T> & operator-=(U const & s)
  1021. {
  1022. this->x -= T(s);
  1023. this->y -= T(s);
  1024. this->z -= T(s);
  1025. this->w -= T(s);
  1026. return *this;
  1027. }
  1028. template <typename U>
  1029. tvec4<T> & operator-=(tvec4<U> const & v)
  1030. {
  1031. this->x -= T(v.x);
  1032. this->y -= T(v.y);
  1033. this->z -= T(v.z);
  1034. this->w -= T(v.w);
  1035. return *this;
  1036. }
  1037. template <typename U>
  1038. tvec4<T> & operator*= (U const & s)
  1039. {
  1040. this->x *= T(s);
  1041. this->y *= T(s);
  1042. this->z *= T(s);
  1043. this->w *= T(s);
  1044. return *this;
  1045. }
  1046. template <typename U>
  1047. tvec4<T> & operator*=( tvec4<U> const & v)
  1048. {
  1049. this->x *= T(v.x);
  1050. this->y *= T(v.y);
  1051. this->z *= T(v.z);
  1052. this->w *= T(v.w);
  1053. return *this;
  1054. }
  1055. template <typename U>
  1056. tvec4<T> & operator/=(U const & s)
  1057. {
  1058. this->x /= T(s);
  1059. this->y /= T(s);
  1060. this->z /= T(s);
  1061. this->w /= T(s);
  1062. return *this;
  1063. }
  1064. template <typename U>
  1065. tvec4<T> & operator/=(tvec4<U> const & v)
  1066. {
  1067. this->x /= T(v.x);
  1068. this->y /= T(v.y);
  1069. this->z /= T(v.z);
  1070. this->w /= T(v.w);
  1071. return *this;
  1072. }
  1073. tvec4<T> & operator++()
  1074. {
  1075. ++this->x;
  1076. ++this->y;
  1077. ++this->z;
  1078. ++this->w;
  1079. return *this;
  1080. }
  1081. tvec4<T> & operator--()
  1082. {
  1083. --this->x;
  1084. --this->y;
  1085. --this->z;
  1086. --this->w;
  1087. return *this;
  1088. }
  1089. };
  1090. template <typename T>
  1091. tvec4<T> rotateX(const tvec4<T>& v, T angle)
  1092. {
  1093. tvec4<T> res(v);
  1094. T c = cos(DEG2RAD(angle));
  1095. T s = sin(DEG2RAD(angle));
  1096. res.y = v.y * c - v.z * s;
  1097. res.z = v.y * s + v.z * c;
  1098. return res;
  1099. }
  1100. template <typename T>
  1101. tvec4<T> rotateY(tvec4<T> const & v, T angle)
  1102. {
  1103. tvec4<T> res = v;
  1104. T c = cos(DEG2RAD(angle));
  1105. T s = sin(DEG2RAD(angle));
  1106. res.x = v.x * c + v.z * s;
  1107. res.z = -v.x * s + v.z * c;
  1108. return res;
  1109. }
  1110. template <typename T>
  1111. tvec4<T> rotateZ(tvec4<T> const & v, T angle)
  1112. {
  1113. tvec4<T> res = v;
  1114. T c = cos(DEG2RAD(angle));
  1115. T s = sin(DEG2RAD(angle));
  1116. res.x = v.x * c - v.y * s;
  1117. res.y = v.x * s + v.y * c;
  1118. return res;
  1119. }
  1120. template <typename T>
  1121. tvec4<T> operator+ (tvec4<T> const & v, T const & s)
  1122. {
  1123. return tvec4<T>(
  1124. v.x + s,
  1125. v.y + s,
  1126. v.z + s,
  1127. v.w + s);
  1128. }
  1129. template <typename T>
  1130. tvec4<T> operator+ (T const & s, tvec4<T> const & v)
  1131. {
  1132. return tvec4<T>(
  1133. s + v.x,
  1134. s + v.y,
  1135. s + v.z,
  1136. s + v.w);
  1137. }
  1138. template <typename T>
  1139. tvec4<T> operator+ (tvec4<T> const & v1, tvec4<T> const & v2)
  1140. {
  1141. return tvec4<T>(
  1142. v1.x + v2.x,
  1143. v1.y + v2.y,
  1144. v1.z + v2.z,
  1145. v1.w + v2.w);
  1146. }
  1147. template <typename T>
  1148. tvec4<T> operator- (tvec4<T> const & v, T const & s)
  1149. {
  1150. return tvec4<T>(
  1151. v.x - s,
  1152. v.y - s,
  1153. v.z - s,
  1154. v.w - s);
  1155. }
  1156. template <typename T>
  1157. tvec4<T> operator- (T const & s, tvec4<T> const & v)
  1158. {
  1159. return tvec4<T>(
  1160. s - v.x,
  1161. s - v.y,
  1162. s - v.z,
  1163. s - v.w);
  1164. }
  1165. template <typename T>
  1166. tvec4<T> operator-
  1167. (
  1168. tvec4<T> const & v1,
  1169. tvec4<T> const & v2
  1170. )
  1171. {
  1172. return tvec4<T>(
  1173. v1.x - v2.x,
  1174. v1.y - v2.y,
  1175. v1.z - v2.z,
  1176. v1.w - v2.w);
  1177. }
  1178. template <typename T>
  1179. tvec4<T> operator* (tvec4<T> const & v, T const & s)
  1180. {
  1181. return tvec4<T>(
  1182. v.x * s,
  1183. v.y * s,
  1184. v.z * s,
  1185. v.w * s);
  1186. }
  1187. template <typename T>
  1188. tvec4<T> operator* (T const & s, tvec4<T> const & v)
  1189. {
  1190. return tvec4<T>(
  1191. s * v.x,
  1192. s * v.y,
  1193. s * v.z,
  1194. s * v.w);
  1195. }
  1196. template <typename T>
  1197. tvec4<T> operator*(tvec4<T> const & v1, tvec4<T> const & v2)
  1198. {
  1199. return tvec4<T>(
  1200. v1.x * v2.x,
  1201. v1.y * v2.y,
  1202. v1.z * v2.z,
  1203. v1.w * v2.w);
  1204. }
  1205. template <typename T>
  1206. tvec4<T> operator/ (tvec4<T> const & v, T const & s)
  1207. {
  1208. return tvec4<T>(
  1209. v.x / s,
  1210. v.y / s,
  1211. v.z / s,
  1212. v.w / s);
  1213. }
  1214. template <typename T>
  1215. tvec4<T> operator/ (T const & s, tvec4<T> const & v)
  1216. {
  1217. return tvec4<T>(
  1218. s / v.x,
  1219. s / v.y,
  1220. s / v.z,
  1221. s / v.w);
  1222. }
  1223. template <typename T>
  1224. tvec4<T> operator/ ( tvec4<T> const & v1, tvec4<T> const & v2)
  1225. {
  1226. return tvec4<T>(
  1227. v1.x / v2.x,
  1228. v1.y / v2.y,
  1229. v1.z / v2.z,
  1230. v1.w / v2.w);
  1231. }
  1232. template <typename T>
  1233. tvec4<T> operator- ( tvec4<T> const & v)
  1234. {
  1235. return tvec4<T>(
  1236. -v.x,
  1237. -v.y,
  1238. -v.z,
  1239. -v.w);
  1240. }
  1241. template <typename T>
  1242. bool operator==
  1243. (
  1244. tvec4<T> const & v1,
  1245. tvec4<T> const & v2
  1246. )
  1247. {
  1248. return (v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z) && (v1.w == v2.w);
  1249. }
  1250. template <typename T>
  1251. bool operator!=(tvec4<T> const & v1, tvec4<T> const & v2)
  1252. {
  1253. return (v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z) || (v1.w != v2.w);
  1254. }
  1255. template<typename T>
  1256. class trect
  1257. {
  1258. public:
  1259. trect(T left = 0,T top = 0,T right = 0,T bottom = 0)
  1260. {
  1261. _left = left;
  1262. _top = top;
  1263. _right = right;
  1264. _bottom = bottom;
  1265. }
  1266. void fromCenter(T x,T y,T size)
  1267. {
  1268. _left = x - size * T(0.5f);
  1269. _top = y - size * T(0.5f);
  1270. _right = x + size * T(0.5f);
  1271. _bottom = y + size * T(0.5f);
  1272. }
  1273. void fromCenter(T x,T y,T sizeX,T sizeY)
  1274. {
  1275. _left = x - sizeX * T(0.5f);
  1276. _top = y - sizeY * T(0.5f);
  1277. _right = x + sizeX * T(0.5f);
  1278. _bottom = y + sizeY * T(0.5f);
  1279. }
  1280. bool ptInRect(T x,T y)
  1281. {
  1282. return x >= _left && x <= _right && y >= _top && y <= _bottom;
  1283. }
  1284. tvec2<T>center() const
  1285. {
  1286. return tvec2<T>((_left + _right) * T(0.5f),(_bottom + _top) * T(0.5f));
  1287. }
  1288. tvec2<T>halSize() const
  1289. {
  1290. return tvec2<T>((_right - _left) * T(0.5f),(_bottom - _top) * T(0.5f));
  1291. }
  1292. public:
  1293. T _left;
  1294. T _top;
  1295. T _right;
  1296. T _bottom;
  1297. };
  1298. template <typename T>
  1299. struct tmat2x2
  1300. {
  1301. typedef T value_type;
  1302. typedef std::size_t size_type;
  1303. typedef tvec2<T> col_type;
  1304. typedef tvec2<T> row_type;
  1305. typedef tmat2x2<T> type;
  1306. typedef tmat2x2<T> transpose_type;
  1307. public:
  1308. tmat2x2<T> _inverse() const
  1309. {
  1310. value_type Determinant = this->value[0][0] * this->value[1][1] - this->value[1][0] * this->value[0][1];
  1311. tmat2x2<T> Inverse(
  1312. + this->value[1][1] / Determinant,
  1313. - this->value[0][1] / Determinant,
  1314. - this->value[1][0] / Determinant,
  1315. + this->value[0][0] / Determinant);
  1316. return Inverse;
  1317. }
  1318. private:
  1319. col_type value[2];
  1320. public:
  1321. size_type length() const
  1322. {
  1323. return 2;
  1324. }
  1325. static size_type col_size()
  1326. {
  1327. return 2;
  1328. }
  1329. static size_type row_size()
  1330. {
  1331. return 2;
  1332. }
  1333. col_type &operator[](size_type i)
  1334. {
  1335. assert(i < this->length());
  1336. return this->value[i];
  1337. }
  1338. col_type const &operator[](size_type i) const
  1339. {
  1340. assert(i < this->length());
  1341. return this->value[i];
  1342. }
  1343. tmat2x2()
  1344. {
  1345. this->value[0] = col_type(1, 0);
  1346. this->value[1] = col_type(0, 1);
  1347. }
  1348. tmat2x2(tmat2x2<T> const & m)
  1349. {
  1350. this->value[0] = m.value[0];
  1351. this->value[1] = m.value[1];
  1352. }
  1353. tmat2x2(value_type s)
  1354. {
  1355. value_type const Zero(0);
  1356. this->value[0] = col_type(s, Zero);
  1357. this->value[1] = col_type(Zero, s);
  1358. }
  1359. tmat2x2(value_type x0, value_type y0, value_type x1, value_type y1)
  1360. {
  1361. this->value[0] = col_type(x0, y0);
  1362. this->value[1] = col_type(x1, y1);
  1363. }
  1364. tmat2x2(col_type const & v0, col_type const & v1)
  1365. {
  1366. this->value[0] = v0;
  1367. this->value[1] = v1;
  1368. }
  1369. template <typename U>
  1370. tmat2x2(U s)
  1371. {
  1372. value_type const Zero(0);
  1373. this->value[0] = tvec2<T>(value_type(s), Zero);
  1374. this->value[1] = tvec2<T>(Zero, value_type(s));
  1375. }
  1376. template <typename X1, typename Y1, typename X2, typename Y2>
  1377. tmat2x2(X1 x1, Y1 y1, X2 x2, Y2 y2)
  1378. {
  1379. this->value[0] = col_type(value_type(x1), value_type(y1));
  1380. this->value[1] = col_type(value_type(x2), value_type(y2));
  1381. }
  1382. template <typename V1, typename V2>
  1383. tmat2x2
  1384. (
  1385. tvec2<V1> const & v1,
  1386. tvec2<V2> const & v2
  1387. )
  1388. {
  1389. this->value[0] = col_type(v1);
  1390. this->value[1] = col_type(v2);
  1391. }
  1392. template <typename U>
  1393. tmat2x2(tmat2x2<U> const & m)
  1394. {
  1395. this->value[0] = col_type(m[0]);
  1396. this->value[1] = col_type(m[1]);
  1397. }
  1398. tmat2x2<T>& operator=(tmat2x2<T> const & m)
  1399. {
  1400. this->value[0] = m[0];
  1401. this->value[1] = m[1];
  1402. return *this;
  1403. }
  1404. template <typename U>
  1405. tmat2x2<T>& operator=
  1406. (
  1407. tmat2x2<U> const & m
  1408. )
  1409. {
  1410. this->value[0] = m[0];
  1411. this->value[1] = m[1];
  1412. return *this;
  1413. }
  1414. template <typename U>
  1415. tmat2x2<T>& operator+=(U const & s)
  1416. {
  1417. this->value[0] += s;
  1418. this->value[1] += s;
  1419. return *this;
  1420. }
  1421. template <typename U>
  1422. tmat2x2<T>& operator+=
  1423. (
  1424. tmat2x2<U> const & m
  1425. )
  1426. {
  1427. this->value[0] += m[0];
  1428. this->value[1] += m[1];
  1429. return *this;
  1430. }
  1431. template <typename U>
  1432. tmat2x2<T>& operator-=(U const & s)
  1433. {
  1434. this->value[0] -= s;
  1435. this->value[1] -= s;
  1436. return *this;
  1437. }
  1438. template <typename U>
  1439. tmat2x2<T>& operator-=(tmat2x2<U> const & m)
  1440. {
  1441. this->value[0] -= m[0];
  1442. this->value[1] -= m[1];
  1443. return *this;
  1444. }
  1445. template <typename U>
  1446. tmat2x2<T>& operator*= ( U const & s)
  1447. {
  1448. this->value[0] *= s;
  1449. this->value[1] *= s;
  1450. return *this;
  1451. }
  1452. template <typename U>
  1453. tmat2x2<T>& operator*= (tmat2x2<U> const & m)
  1454. {
  1455. return (*this = *this * m);
  1456. }
  1457. template <typename U>
  1458. tmat2x2<T>& operator/= (U const & s)
  1459. {
  1460. this->value[0] /= s;
  1461. this->value[1] /= s;
  1462. return *this;
  1463. }
  1464. template <typename U>
  1465. tmat2x2<T>& operator/= (tmat2x2<U> const & m)
  1466. {
  1467. return (*this = *this / m);
  1468. }
  1469. tmat2x2<T>& operator++ ()
  1470. {
  1471. ++this->value[0];
  1472. ++this->value[1];
  1473. return *this;
  1474. }
  1475. tmat2x2<T>& operator-- ()
  1476. {
  1477. --this->value[0];
  1478. --this->value[1];
  1479. return *this;
  1480. };
  1481. };
  1482. template <typename T>
  1483. tmat2x2<T> rotate(T angle)
  1484. {
  1485. T c = cos(DEG2RAD(angle));
  1486. T s = sin(DEG2RAD(angle));
  1487. return tmat2x2<T>(c,s,-s,c);
  1488. }
  1489. template <typename T>
  1490. tmat2x2<T> operator+ (tmat2x2<T> const & m, T const & s)
  1491. {
  1492. return tmat2x2<T>(m[0] + s,m[1] + s);
  1493. }
  1494. template <typename T>
  1495. tmat2x2<T> operator+ (T const & s, tmat2x2<T> const & m)
  1496. {
  1497. return tmat2x2<T>(m[0] + s,m[1] + s);
  1498. }
  1499. template <typename T>
  1500. tmat2x2<T> operator+ (tmat2x2<T> const & m1, tmat2x2<T> const & m2)
  1501. {
  1502. return tmat2x2<T>(m1[0] + m2[0],m1[1] + m2[1]);
  1503. }
  1504. template <typename T>
  1505. tmat2x2<T> operator- (tmat2x2<T> const & m, T const & s)
  1506. {
  1507. return tmat2x2<T>(m[0] - s,m[1] - s);
  1508. }
  1509. template <typename T>
  1510. tmat2x2<T> operator- (T const & s, tmat2x2<T> const & m)
  1511. {
  1512. return tmat2x2<T>(s - m[0],s - m[1]);
  1513. }
  1514. template <typename T>
  1515. tmat2x2<T> operator- (tmat2x2<T> const & m1, tmat2x2<T> const & m2)
  1516. {
  1517. return tmat2x2<T>(m1[0] - m2[0],m1[1] - m2[1]);
  1518. }
  1519. template <typename T>
  1520. tmat2x2<T> operator* (tmat2x2<T> const & m, T const & s)
  1521. {
  1522. return tmat2x2<T>(m[0] * s,m[1] * s);
  1523. }
  1524. template <typename T>
  1525. tmat2x2<T> operator* ( T const & s, tmat2x2<T> const & m)
  1526. {
  1527. return tmat2x2<T>(m[0] * s,m[1] * s);
  1528. }
  1529. template <typename T>
  1530. tvec2<T> operator*(tmat2x2<T> const & m, tvec2<T> const & v)
  1531. {
  1532. return tvec2<T>(
  1533. m[0][0] * v.x + m[1][0] * v.y,
  1534. m[0][1] * v.x + m[1][1] * v.y);
  1535. }
  1536. template <typename T>
  1537. tvec2<T> operator*(tvec2<T> const & v, tmat2x2<T> const & m)
  1538. {
  1539. return tvec2<T>(
  1540. v.x * m[0][0] + v.y * m[0][1],
  1541. v.x * m[1][0] + v.y * m[1][1]);
  1542. }
  1543. template <typename T>
  1544. tmat2x2<T> operator*(tmat2x2<T> const & m1,tmat2x2<T> const & m2)
  1545. {
  1546. return tmat2x2<T>(
  1547. m1[0][0] * m2[0][0] + m1[1][0] * m2[0][1],
  1548. m1[0][1] * m2[0][0] + m1[1][1] * m2[0][1],
  1549. m1[0][0] * m2[1][0] + m1[1][0] * m2[1][1],
  1550. m1[0][1] * m2[1][0] + m1[1][1] * m2[1][1]);
  1551. }
  1552. template <typename T>
  1553. struct tmat3x3
  1554. {
  1555. typedef T value_type;
  1556. typedef std::size_t size_type;
  1557. typedef tvec3<T> col_type;
  1558. typedef tvec3<T> row_type;
  1559. typedef tmat3x3<T> type;
  1560. typedef tmat3x3<T> transpose_type;
  1561. private:
  1562. // Data
  1563. col_type value[3];
  1564. public:
  1565. size_type length() const
  1566. {
  1567. return 3;
  1568. }
  1569. size_type col_size()
  1570. {
  1571. return 3;
  1572. }
  1573. size_type row_size()
  1574. {
  1575. return 3;
  1576. }
  1577. tmat3x3()
  1578. {
  1579. value_type const Zero(0);
  1580. value_type const One(1);
  1581. this->value[0] = col_type(One, Zero, Zero);
  1582. this->value[1] = col_type(Zero, One, Zero);
  1583. this->value[2] = col_type(Zero, Zero, One);
  1584. }
  1585. tmat3x3
  1586. (
  1587. tmat3x3<T> const & m
  1588. )
  1589. {
  1590. this->value[0] = m.value[0];
  1591. this->value[1] = m.value[1];
  1592. this->value[2] = m.value[2];
  1593. }
  1594. tmat3x3(value_type const & s)
  1595. {
  1596. value_type const Zero(0);
  1597. this->value[0] = col_type(s, Zero, Zero);
  1598. this->value[1] = col_type(Zero, s, Zero);
  1599. this->value[2] = col_type(Zero, Zero, s);
  1600. }
  1601. tmat3x3
  1602. (
  1603. value_type const & x0, value_type const & y0, value_type const & z0,
  1604. value_type const & x1, value_type const & y1, value_type const & z1,
  1605. value_type const & x2, value_type const & y2, value_type const & z2
  1606. )
  1607. {
  1608. this->value[0] = col_type(x0, y0, z0);
  1609. this->value[1] = col_type(x1, y1, z1);
  1610. this->value[2] = col_type(x2, y2, z2);
  1611. }
  1612. tmat3x3
  1613. (
  1614. col_type const & v0,
  1615. col_type const & v1,
  1616. col_type const & v2
  1617. )
  1618. {
  1619. this->value[0] = v0;
  1620. this->value[1] = v1;
  1621. this->value[2] = v2;
  1622. }
  1623. template <typename U>
  1624. tmat3x3(U const & s)
  1625. {
  1626. value_type const Zero(0);
  1627. this->value[0] = tvec3<T>(value_type(s), Zero, Zero);
  1628. this->value[1] = tvec3<T>(Zero, value_type(s), Zero);
  1629. this->value[2] = tvec3<T>(Zero, Zero, value_type(s));
  1630. }
  1631. template <
  1632. typename X1, typename Y1, typename Z1,
  1633. typename X2, typename Y2, typename Z2,
  1634. typename X3, typename Y3, typename Z3>
  1635. tmat3x3
  1636. (
  1637. X1 const & x1, Y1 const & y1, Z1 const & z1,
  1638. X2 const & x2, Y2 const & y2, Z2 const & z2,
  1639. X3 const & x3, Y3 const & y3, Z3 const & z3
  1640. )
  1641. {
  1642. this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1));
  1643. this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2));
  1644. this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3));
  1645. }
  1646. template <typename V1, typename V2, typename V3>
  1647. tmat3x3
  1648. (
  1649. tvec3<V1> const & v1,
  1650. tvec3<V2> const & v2,
  1651. tvec3<V3> const & v3
  1652. )
  1653. {
  1654. this->value[0] = col_type(v1);
  1655. this->value[1] = col_type(v2);
  1656. this->value[2] = col_type(v3);
  1657. }
  1658. template <typename U>
  1659. tmat3x3(tmat3x3<U> const & m)
  1660. {
  1661. this->value[0] = col_type(m[0]);
  1662. this->value[1] = col_type(m[1]);
  1663. this->value[2] = col_type(m[2]);
  1664. }
  1665. tmat3x3<T> _inverse() const
  1666. {
  1667. T S00 = value[0][0];
  1668. T S01 = value[0][1];
  1669. T S02 = value[0][2];
  1670. T S10 = value[1][0];
  1671. T S11 = value[1][1];
  1672. T S12 = value[1][2];
  1673. T S20 = value[2][0];
  1674. T S21 = value[2][1];
  1675. T S22 = value[2][2];
  1676. tmat3x3<T> Inverse(
  1677. S11 * S22 - S21 * S12,
  1678. S12 * S20 - S22 * S10,
  1679. S10 * S21 - S20 * S11,
  1680. S02 * S21 - S01 * S22,
  1681. S00 * S22 - S02 * S20,
  1682. S01 * S20 - S00 * S21,
  1683. S12 * S01 - S11 * S02,
  1684. S10 * S02 - S12 * S00,
  1685. S11 * S00 - S10 * S01);
  1686. T Determinant = S00 * (S11 * S22 - S21 * S12)
  1687. - S10 * (S01 * S22 - S21 * S02)
  1688. + S20 * (S01 * S12 - S11 * S02);
  1689. Inverse /= Determinant;
  1690. return Inverse;
  1691. }
  1692. col_type & operator[](size_type i)
  1693. {
  1694. assert(i < this->length());
  1695. return this->value[i];
  1696. }
  1697. col_type const & operator[](size_type i) const
  1698. {
  1699. assert(i < this->length());
  1700. return this->value[i];
  1701. }
  1702. tmat3x3<T> & operator=(tmat3x3<T> const & m)
  1703. {
  1704. this->value[0] = m[0];
  1705. this->value[1] = m[1];
  1706. this->value[2] = m[2];
  1707. return *this;
  1708. }
  1709. template <typename U>
  1710. tmat3x3<T> & operator=(tmat3x3<U> const & m)
  1711. {
  1712. this->value[0] = m[0];
  1713. this->value[1] = m[1];
  1714. this->value[2] = m[2];
  1715. return *this;
  1716. }
  1717. template <typename U>
  1718. tmat3x3<T> & operator+= (U const & s)
  1719. {
  1720. this->value[0] += s;
  1721. this->value[1] += s;
  1722. this->value[2] += s;
  1723. return *this;
  1724. }
  1725. template <typename U>
  1726. tmat3x3<T> & operator+=(tmat3x3<U> const & m)
  1727. {
  1728. this->value[0] += m[0];
  1729. this->value[1] += m[1];
  1730. this->value[2] += m[2];
  1731. return *this;
  1732. }
  1733. template <typename U>
  1734. tmat3x3<T> & operator-= (U const & s)
  1735. {
  1736. this->value[0] -= s;
  1737. this->value[1] -= s;
  1738. this->value[2] -= s;
  1739. return *this;
  1740. }
  1741. template <typename U>
  1742. tmat3x3<T> & operator-= (tmat3x3<U> const & m)
  1743. {
  1744. this->value[0] -= m[0];
  1745. this->value[1] -= m[1];
  1746. this->value[2] -= m[2];
  1747. return *this;
  1748. }
  1749. template <typename U>
  1750. tmat3x3<T> & operator*= (U const & s)
  1751. {
  1752. this->value[0] *= s;
  1753. this->value[1] *= s;
  1754. this->value[2] *= s;
  1755. return *this;
  1756. }
  1757. template <typename U>
  1758. tmat3x3<T> & operator*= (tmat3x3<U> const & m)
  1759. {
  1760. return (*this = *this * m);
  1761. }
  1762. template <typename U>
  1763. tmat3x3<T> & operator/= (U const & s)
  1764. {
  1765. this->value[0] /= s;
  1766. this->value[1] /= s;
  1767. this->value[2] /= s;
  1768. return *this;
  1769. }
  1770. template <typename U>
  1771. tmat3x3<T> & operator/= (tmat3x3<U> const & m)
  1772. {
  1773. return (*this = *this / m);
  1774. }
  1775. tmat3x3<T> & operator++ ()
  1776. {
  1777. ++this->value[0];
  1778. ++this->value[1];
  1779. ++this->value[2];
  1780. return *this;
  1781. }
  1782. tmat3x3<T> & operator-- ()
  1783. {
  1784. --this->value[0];
  1785. --this->value[1];
  1786. --this->value[2];
  1787. return *this;
  1788. }
  1789. };
  1790. template <typename T>
  1791. tmat3x3<T> operator+ (tmat3x3<T> const & m, T const & s)
  1792. {
  1793. return tmat3x3<T>(
  1794. m[0] + s,
  1795. m[1] + s,
  1796. m[2] + s);
  1797. }
  1798. template <typename T>
  1799. tmat3x3<T> operator+ (T const & s, tmat3x3<T> const & m)
  1800. {
  1801. return tmat3x3<T>(
  1802. m[0] + s,
  1803. m[1] + s,
  1804. m[2] + s);
  1805. }
  1806. template <typename T>
  1807. tmat3x3<T> operator+ (tmat3x3<T> const & m1, tmat3x3<T> const & m2)
  1808. {
  1809. return tmat3x3<T>(
  1810. m1[0] + m2[0],
  1811. m1[1] + m2[1],
  1812. m1[2] + m2[2]);
  1813. }
  1814. template <typename T>
  1815. tmat3x3<T> operator- (tmat3x3<T> const & m, T const & s)
  1816. {
  1817. return tmat3x3<T>(
  1818. m[0] - s,
  1819. m[1] - s,
  1820. m[2] - s);
  1821. }
  1822. template <typename T>
  1823. tmat3x3<T> operator- (T const & s, tmat3x3<T> const & m)
  1824. {
  1825. return tmat3x3<T>(
  1826. s - m[0],
  1827. s - m[1],
  1828. s - m[2]);
  1829. }
  1830. template <typename T>
  1831. tmat3x3<T> operator- (tmat3x3<T> const & m1, tmat3x3<T> const & m2)
  1832. {
  1833. return tmat3x3<T>(
  1834. m1[0] - m2[0],
  1835. m1[1] - m2[1],
  1836. m1[2] - m2[2]);
  1837. }
  1838. template <typename T>
  1839. tmat3x3<T> operator* (tmat3x3<T> const & m, T const & s)
  1840. {
  1841. return tmat3x3<T>(
  1842. m[0] * s,
  1843. m[1] * s,
  1844. m[2] * s);
  1845. }
  1846. template <typename T>
  1847. tmat3x3<T> operator* (T const & s, tmat3x3<T> const & m)
  1848. {
  1849. return tmat3x3<T>(
  1850. m[0] * s,
  1851. m[1] * s,
  1852. m[2] * s);
  1853. }
  1854. template <typename T>
  1855. tvec3<T> operator* (tmat3x3<T> const & m, tvec3<T> const & v)
  1856. {
  1857. return tvec3<T>(
  1858. m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z,
  1859. m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z,
  1860. m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z);
  1861. }
  1862. template <typename T>
  1863. tvec3<T> operator* (tvec3<T> const & v, tmat3x3<T> const & m)
  1864. {
  1865. return tvec3<T>(
  1866. m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z,
  1867. m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z,
  1868. m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z);
  1869. }
  1870. template <typename T>
  1871. tmat3x3<T> operator* (tmat3x3<T> const & m1, tmat3x3<T> const & m2)
  1872. {
  1873. T const srcA00 = m1[0][0];
  1874. T const srcA01 = m1[0][1];
  1875. T const srcA02 = m1[0][2];
  1876. T const srcA10 = m1[1][0];
  1877. T const srcA11 = m1[1][1];
  1878. T const srcA12 = m1[1][2];
  1879. T const srcA20 = m1[2][0];
  1880. T const srcA21 = m1[2][1];
  1881. T const srcA22 = m1[2][2];
  1882. T const srcB00 = m2[0][0];
  1883. T const srcB01 = m2[0][1];
  1884. T const srcB02 = m2[0][2];
  1885. T const srcB10 = m2[1][0];
  1886. T const srcB11 = m2[1][1];
  1887. T const srcB12 = m2[1][2];
  1888. T const srcB20 = m2[2][0];
  1889. T const srcB21 = m2[2][1];
  1890. T const srcB22 = m2[2][2];
  1891. tmat3x3<T> res;
  1892. res[0][0] = srcA00 * srcB00 + srcA10 * srcB01 + srcA20 * srcB02;
  1893. res[0][1] = srcA01 * srcB00 + srcA11 * srcB01 + srcA21 * srcB02;
  1894. res[0][2] = srcA02 * srcB00 + srcA12 * srcB01 + srcA22 * srcB02;
  1895. res[1][0] = srcA00 * srcB10 + srcA10 * srcB11 + srcA20 * srcB12;
  1896. res[1][1] = srcA01 * srcB10 + srcA11 * srcB11 + srcA21 * srcB12;
  1897. res[1][2] = srcA02 * srcB10 + srcA12 * srcB11 + srcA22 * srcB12;
  1898. res[2][0] = srcA00 * srcB20 + srcA10 * srcB21 + srcA20 * srcB22;
  1899. res[2][1] = srcA01 * srcB20 + srcA11 * srcB21 + srcA21 * srcB22;
  1900. res[2][2] = srcA02 * srcB20 + srcA12 * srcB21 + srcA22 * srcB22;
  1901. return res;
  1902. }
  1903. template <typename T>
  1904. tmat3x3<T> operator/ (tmat3x3<T> const & m, T const & s)
  1905. {
  1906. return tmat3x3<T>(
  1907. m[0] / s,
  1908. m[1] / s,
  1909. m[2] / s);
  1910. }
  1911. template <typename T>
  1912. tmat3x3<T> operator/ (T const & s, tmat3x3<T> const & m)
  1913. {
  1914. return tmat3x3<T>(
  1915. s / m[0],
  1916. s / m[1],
  1917. s / m[2]
  1918. );
  1919. }
  1920. template <typename T>
  1921. tvec3<T> operator/ (tmat3x3<T> const & m, tvec3<T> const & v)
  1922. {
  1923. return m._inverse() * v;
  1924. }
  1925. template <typename T>
  1926. tvec3<T> operator/ (tvec3<T> const & v, tmat3x3<T> const & m)
  1927. {
  1928. return v * m._inverse();
  1929. }
  1930. template <typename T>
  1931. tmat3x3<T> operator/ (tmat3x3<T> const & m1, tmat3x3<T> const & m2)
  1932. {
  1933. return m1 * m2._inverse();
  1934. }
  1935. template <typename T>
  1936. tmat3x3<T> const operator- (tmat3x3<T> const & m)
  1937. {
  1938. return tmat3x3<T>(
  1939. -m[0],
  1940. -m[1],
  1941. -m[2]);
  1942. }
  1943. template <typename T>
  1944. tmat3x3<T> const operator++ (tmat3x3<T> const & m, int)
  1945. {
  1946. return tmat3x3<T>(
  1947. m[0] + T(1),
  1948. m[1] + T(1),
  1949. m[2] + T(1));
  1950. }
  1951. template <typename T>
  1952. tmat3x3<T> const operator-- (tmat3x3<T> const & m, int)
  1953. {
  1954. return tmat3x3<T>(
  1955. m[0] - T(1),
  1956. m[1] - T(1),
  1957. m[2] - T(1));
  1958. }
  1959. template <typename T>
  1960. bool operator==(tmat3x3<T> const & m1, tmat3x3<T> const & m2)
  1961. {
  1962. return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]);
  1963. }
  1964. template <typename T>
  1965. bool operator!=(tmat3x3<T> const & m1, tmat3x3<T> const & m2)
  1966. {
  1967. return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]);
  1968. }
  1969. template <typename T>
  1970. struct tmat4x4
  1971. {
  1972. typedef T value_type;
  1973. typedef std::size_t size_type;
  1974. typedef tvec4<T> col_type;
  1975. typedef tvec4<T> row_type;
  1976. typedef tmat4x4<T> type;
  1977. typedef tmat4x4<T> transpose_type;
  1978. public:
  1979. tmat4x4<T> inverse() const
  1980. {
  1981. value_type subFactor00 = this->value[2][2] * this->value[3][3] - this->value[3][2] * this->value[2][3];
  1982. value_type subFactor01 = this->value[2][1] * this->value[3][3] - this->value[3][1] * this->value[2][3];
  1983. value_type subFactor02 = this->value[2][1] * this->value[3][2] - this->value[3][1] * this->value[2][2];
  1984. value_type subFactor03 = this->value[2][0] * this->value[3][3] - this->value[3][0] * this->value[2][3];
  1985. value_type subFactor04 = this->value[2][0] * this->value[3][2] - this->value[3][0] * this->value[2][2];
  1986. value_type subFactor05 = this->value[2][0] * this->value[3][1] - this->value[3][0] * this->value[2][1];
  1987. value_type subFactor06 = this->value[1][2] * this->value[3][3] - this->value[3][2] * this->value[1][3];
  1988. value_type subFactor07 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3];
  1989. value_type subFactor08 = this->value[1][1] * this->value[3][2] - this->value[3][1] * this->value[1][2];
  1990. value_type subFactor09 = this->value[1][0] * this->value[3][3] - this->value[3][0] * this->value[1][3];
  1991. value_type subFactor10 = this->value[1][0] * this->value[3][2] - this->value[3][0] * this->value[1][2];
  1992. value_type subFactor11 = this->value[1][1] * this->value[3][3] - this->value[3][1] * this->value[1][3];
  1993. value_type SubFactor12 = this->value[1][0] * this->value[3][1] - this->value[3][0] * this->value[1][1];
  1994. value_type subFactor13 = this->value[1][2] * this->value[2][3] - this->value[2][2] * this->value[1][3];
  1995. value_type subFactor14 = this->value[1][1] * this->value[2][3] - this->value[2][1] * this->value[1][3];
  1996. value_type subFactor15 = this->value[1][1] * this->value[2][2] - this->value[2][1] * this->value[1][2];
  1997. value_type subFactor16 = this->value[1][0] * this->value[2][3] - this->value[2][0] * this->value[1][3];
  1998. value_type subFactor17 = this->value[1][0] * this->value[2][2] - this->value[2][0] * this->value[1][2];
  1999. value_type subFactor18 = this->value[1][0] * this->value[2][1] - this->value[2][0] * this->value[1][1];
  2000. tmat4x4<T> res(
  2001. + this->value[1][1] * subFactor00 - this->value[1][2] * subFactor01 + this->value[1][3] * subFactor02,
  2002. - this->value[1][0] * subFactor00 + this->value[1][2] * subFactor03 - this->value[1][3] * subFactor04,
  2003. + this->value[1][0] * subFactor01 - this->value[1][1] * subFactor03 + this->value[1][3] * subFactor05,
  2004. - this->value[1][0] * subFactor02 + this->value[1][1] * subFactor04 - this->value[1][2] * subFactor05,
  2005. - this->value[0][1] * subFactor00 + this->value[0][2] * subFactor01 - this->value[0][3] * subFactor02,
  2006. + this->value[0][0] * subFactor00 - this->value[0][2] * subFactor03 + this->value[0][3] * subFactor04,
  2007. - this->value[0][0] * subFactor01 + this->value[0][1] * subFactor03 - this->value[0][3] * subFactor05,
  2008. + this->value[0][0] * subFactor02 - this->value[0][1] * subFactor04 + this->value[0][2] * subFactor05,
  2009. + this->value[0][1] * subFactor06 - this->value[0][2] * subFactor07 + this->value[0][3] * subFactor08,
  2010. - this->value[0][0] * subFactor06 + this->value[0][2] * subFactor09 - this->value[0][3] * subFactor10,
  2011. + this->value[0][0] * subFactor11 - this->value[0][1] * subFactor09 + this->value[0][3] * SubFactor12,
  2012. - this->value[0][0] * subFactor08 + this->value[0][1] * subFactor10 - this->value[0][2] * SubFactor12,
  2013. - this->value[0][1] * subFactor13 + this->value[0][2] * subFactor14 - this->value[0][3] * subFactor15,
  2014. + this->value[0][0] * subFactor13 - this->value[0][2] * subFactor16 + this->value[0][3] * subFactor17,
  2015. - this->value[0][0] * subFactor14 + this->value[0][1] * subFactor16 - this->value[0][3] * subFactor18,
  2016. + this->value[0][0] * subFactor15 - this->value[0][1] * subFactor17 + this->value[0][2] * subFactor18);
  2017. value_type determinant =
  2018. + this->value[0][0] * res[0][0]
  2019. + this->value[0][1] * res[1][0]
  2020. + this->value[0][2] * res[2][0]
  2021. + this->value[0][3] * res[3][0];
  2022. res /= determinant;
  2023. return res;
  2024. }
  2025. private:
  2026. col_type value[4];
  2027. public:
  2028. size_type length() const
  2029. {
  2030. return 4;
  2031. }
  2032. size_type col_size()
  2033. {
  2034. return 4;
  2035. }
  2036. size_type row_size()
  2037. {
  2038. return 4;
  2039. }
  2040. void identify()
  2041. {
  2042. this->value[0] = col_type(1, 0, 0, 0);
  2043. this->value[1] = col_type(0, 1, 0, 0);
  2044. this->value[2] = col_type(0, 0, 1, 0);
  2045. this->value[3] = col_type(0, 0, 0, 1);
  2046. }
  2047. col_type & operator[](size_type i)
  2048. {
  2049. assert(i < this->length());
  2050. return this->value[i];
  2051. }
  2052. col_type const & operator[](size_type i) const
  2053. {
  2054. assert(i < this->length());
  2055. return this->value[i];
  2056. }
  2057. tmat4x4(tmat4x4<T> const & m)
  2058. {
  2059. this->value[0] = m.value[0];
  2060. this->value[1] = m.value[1];
  2061. this->value[2] = m.value[2];
  2062. this->value[3] = m.value[3];
  2063. }
  2064. tmat4x4(tmat3x3<T> const & m)
  2065. {
  2066. this->value[0] = col_type(m[0], value_type(0));
  2067. this->value[1] = col_type(m[1], value_type(0));
  2068. this->value[2] = col_type(m[2], value_type(0));
  2069. this->value[3] = col_type(value_type(0), value_type(0), value_type(0), value_type(1));
  2070. }
  2071. tmat4x4()
  2072. {
  2073. }
  2074. tmat4x4(value_type s)
  2075. {
  2076. value_type const Zero(0);
  2077. this->value[0] = col_type(s, Zero, Zero, Zero);
  2078. this->value[1] = col_type(Zero, s, Zero, Zero);
  2079. this->value[2] = col_type(Zero, Zero, s, Zero);
  2080. this->value[3] = col_type(Zero, Zero, Zero, s);
  2081. }
  2082. tmat4x4
  2083. (
  2084. value_type const & x0, value_type const & y0, value_type const & z0, value_type const & w0,
  2085. value_type const & x1, value_type const & y1, value_type const & z1, value_type const & w1,
  2086. value_type const & x2, value_type const & y2, value_type const & z2, value_type const & w2,
  2087. value_type const & x3, value_type const & y3, value_type const & z3, value_type const & w3
  2088. )
  2089. {
  2090. this->value[0] = col_type(x0, y0, z0, w0);
  2091. this->value[1] = col_type(x1, y1, z1, w1);
  2092. this->value[2] = col_type(x2, y2, z2, w2);
  2093. this->value[3] = col_type(x3, y3, z3, w3);
  2094. }
  2095. tmat4x4
  2096. (
  2097. col_type const & v0,
  2098. col_type const & v1,
  2099. col_type const & v2,
  2100. col_type const & v3
  2101. )
  2102. {
  2103. this->value[0] = v0;
  2104. this->value[1] = v1;
  2105. this->value[2] = v2;
  2106. this->value[3] = v3;
  2107. }
  2108. template <typename U>
  2109. tmat4x4(tmat4x4<U> const & m)
  2110. {
  2111. this->value[0] = col_type(m[0]);
  2112. this->value[1] = col_type(m[1]);
  2113. this->value[2] = col_type(m[2]);
  2114. this->value[3] = col_type(m[3]);
  2115. }
  2116. template <typename U>
  2117. tmat4x4(U const & s)
  2118. {
  2119. value_type const Zero(0);
  2120. this->value[0] = tvec4<T>(value_type(s), Zero, Zero, Zero);
  2121. this->value[1] = tvec4<T>(Zero, value_type(s), Zero, Zero);
  2122. this->value[2] = tvec4<T>(Zero, Zero, value_type(s), Zero);
  2123. this->value[3] = tvec4<T>(Zero, Zero, Zero, value_type(s));
  2124. }
  2125. template <
  2126. typename X1, typename Y1, typename Z1, typename W1,
  2127. typename X2, typename Y2, typename Z2, typename W2,
  2128. typename X3, typename Y3, typename Z3, typename W3,
  2129. typename X4, typename Y4, typename Z4, typename W4>
  2130. tmat4x4
  2131. (
  2132. X1 const & x1, Y1 const & y1, Z1 const & z1, W1 const & w1,
  2133. X2 const & x2, Y2 const & y2, Z2 const & z2, W2 const & w2,
  2134. X3 const & x3, Y3 const & y3, Z3 const & z3, W3 const & w3,
  2135. X4 const & x4, Y4 const & y4, Z4 const & z4, W4 const & w4
  2136. )
  2137. {
  2138. this->value[0] = col_type(value_type(x1), value_type(y1), value_type(z1), value_type(w1));
  2139. this->value[1] = col_type(value_type(x2), value_type(y2), value_type(z2), value_type(w2));
  2140. this->value[2] = col_type(value_type(x3), value_type(y3), value_type(z3), value_type(w3));
  2141. this->value[3] = col_type(value_type(x4), value_type(y4), value_type(z4), value_type(w4));
  2142. }
  2143. template <typename V1, typename V2, typename V3, typename V4>
  2144. tmat4x4
  2145. (
  2146. tvec4<V1> const & v1,
  2147. tvec4<V2> const & v2,
  2148. tvec4<V3> const & v3,
  2149. tvec4<V4> const & v4
  2150. )
  2151. {
  2152. this->value[0] = col_type(v1);
  2153. this->value[1] = col_type(v2);
  2154. this->value[2] = col_type(v3);
  2155. this->value[3] = col_type(v4);
  2156. }
  2157. T const * data() const
  2158. {
  2159. return &this->value[0][0];
  2160. }
  2161. tmat4x4<T>& operator= (tmat4x4<T> const & m)
  2162. {
  2163. this->value[0] = m[0];
  2164. this->value[1] = m[1];
  2165. this->value[2] = m[2];
  2166. this->value[3] = m[3];
  2167. return *this;
  2168. }
  2169. template <typename U>
  2170. tmat4x4<T>& operator= (tmat4x4<U> const & m)
  2171. {
  2172. this->value[0] = m[0];
  2173. this->value[1] = m[1];
  2174. this->value[2] = m[2];
  2175. this->value[3] = m[3];
  2176. return *this;
  2177. }
  2178. template <typename U>
  2179. tmat4x4<T>& operator+= (U const & s)
  2180. {
  2181. this->value[0] += s;
  2182. this->value[1] += s;
  2183. this->value[2] += s;
  2184. this->value[3] += s;
  2185. return *this;
  2186. }
  2187. template <typename U>
  2188. tmat4x4<T>& operator+= (tmat4x4<U> const & m)
  2189. {
  2190. this->value[0] += m[0];
  2191. this->value[1] += m[1];
  2192. this->value[2] += m[2];
  2193. this->value[3] += m[3];
  2194. return *this;
  2195. }
  2196. template <typename U>
  2197. tmat4x4<T> & operator-= (U const & s)
  2198. {
  2199. this->value[0] -= s;
  2200. this->value[1] -= s;
  2201. this->value[2] -= s;
  2202. this->value[3] -= s;
  2203. return *this;
  2204. }
  2205. template <typename U>
  2206. tmat4x4<T> & operator-= (tmat4x4<U> const & m)
  2207. {
  2208. this->value[0] -= m[0];
  2209. this->value[1] -= m[1];
  2210. this->value[2] -= m[2];
  2211. this->value[3] -= m[3];
  2212. return *this;
  2213. }
  2214. template <typename U>
  2215. tmat4x4<T> & operator*= (U const & s)
  2216. {
  2217. this->value[0] *= s;
  2218. this->value[1] *= s;
  2219. this->value[2] *= s;
  2220. this->value[3] *= s;
  2221. return *this;
  2222. }
  2223. template <typename U>
  2224. tmat4x4<T> & operator*= (tmat4x4<U> const & m)
  2225. {
  2226. return (*this = *this * m);
  2227. }
  2228. template <typename U>
  2229. tmat4x4<T> & operator/= (U const & s)
  2230. {
  2231. this->value[0] /= s;
  2232. this->value[1] /= s;
  2233. this->value[2] /= s;
  2234. this->value[3] /= s;
  2235. return *this;
  2236. }
  2237. template <typename U>
  2238. tmat4x4<T> & operator/= (tmat4x4<U> const & m)
  2239. {
  2240. return (*this = *this / m);
  2241. }
  2242. tmat4x4<T> & operator++ ()
  2243. {
  2244. ++this->value[0];
  2245. ++this->value[1];
  2246. ++this->value[2];
  2247. ++this->value[3];
  2248. return *this;
  2249. }
  2250. tmat4x4<T> & operator-- ()
  2251. {
  2252. --this->value[0];
  2253. --this->value[1];
  2254. --this->value[2];
  2255. --this->value[3];
  2256. return *this;
  2257. }
  2258. tmat4x4<T>& translate( value_type x,value_type y,value_type z)
  2259. {
  2260. this->value[0] = col_type(1, 0, 0, 0);
  2261. this->value[1] = col_type(0, 1, 0, 0);
  2262. this->value[2] = col_type(0, 0, 1, 0);
  2263. this->value[3] = col_type(x, y, z, 1);
  2264. return *this;
  2265. }
  2266. template<typename U>
  2267. tmat4x4<T>& translate( U x,U y,U z)
  2268. {
  2269. this->value[0] = col_type(1, 0, 0, 0);
  2270. this->value[1] = col_type(0, 1, 0, 0);
  2271. this->value[2] = col_type(0, 0, 1, 0);
  2272. this->value[3] = col_type(T(x), T(y), T(z), 1);
  2273. return *this;
  2274. }
  2275. tmat4x4<T>& translate(tvec3<T> const& pos)
  2276. {
  2277. this->value[0] = col_type(1, 0, 0, 0);
  2278. this->value[1] = col_type(0, 1, 0, 0);
  2279. this->value[2] = col_type(0, 0, 1, 0);
  2280. this->value[3] = col_type(pos.x,pos.y, pos.z, 1);
  2281. return *this;
  2282. }
  2283. template<typename U>
  2284. tmat4x4<T>& translate(tvec3<U> const& pos)
  2285. {
  2286. this->value[0] = col_type(1, 0, 0, 0);
  2287. this->value[1] = col_type(0, 1, 0, 0);
  2288. this->value[2] = col_type(0, 0, 1, 0);
  2289. this->value[3] = col_type(T(pos.x),T(pos.y), T(pos.z), 1);
  2290. return *this;
  2291. }
  2292. tmat4x4<T>& rotate(value_type angle,tvec3<T> const & v )
  2293. {
  2294. T a = DEG2RAD(angle);
  2295. T c = cos(a);
  2296. T s = sin(a);
  2297. tvec3<T> axis = normalize(v);
  2298. tvec3<T> temp = (T(1) - c) * axis;
  2299. tmat4x4<T> res;
  2300. this->value[0][0] = c + temp[0] * axis[0];
  2301. this->value[0][1] = 0 + temp[0] * axis[1] + s * axis[2];
  2302. this->value[0][2] = 0 + temp[0] * axis[2] - s * axis[1];
  2303. this->value[0][3] = 0;
  2304. this->value[1][0] = 0 + temp[1] * axis[0] - s * axis[2];
  2305. this->value[1][1] = c + temp[1] * axis[1];
  2306. this->value[1][2] = 0 + temp[1] * axis[2] + s * axis[0];
  2307. this->value[1][3] = 0;
  2308. this->value[2][0] = 0 + temp[2] * axis[0] + s * axis[1];
  2309. this->value[2][1] = 0 + temp[2] * axis[1] - s * axis[0];
  2310. this->value[2][2] = c + temp[2] * axis[2];
  2311. this->value[2][3] = 0;
  2312. this->value[3][0] = 0;
  2313. this->value[3][1] = 0;
  2314. this->value[3][2] = 0;
  2315. this->value[3][3] = 1;
  2316. return *this;
  2317. }
  2318. tmat4x4<T>& rotateX(value_type angle)
  2319. {
  2320. T a = DEG2RAD(angle);
  2321. T c = cos(a);
  2322. T s = sin(a);
  2323. this->value[0] = col_type(1, 0, 0, 0);
  2324. this->value[1] = col_type(0, c, s, 0);
  2325. this->value[2] = col_type(0,-s, c, 0);
  2326. this->value[3] = col_type(0, 0, 0, 1);
  2327. return *this;
  2328. }
  2329. template<typename U>
  2330. tmat4x4<T>& rotateX(U angle)
  2331. {
  2332. T a = DEG2RAD(angle);
  2333. T c = cos(a);
  2334. T s = sin(a);
  2335. this->value[0] = col_type(1, 0, 0, 0);
  2336. this->value[1] = col_type(0, c, s, 0);
  2337. this->value[2] = col_type(0,-s, c, 0);
  2338. this->value[3] = col_type(0, 0, 0, 1);
  2339. return *this;
  2340. }
  2341. tmat4x4<T>& rotateY(value_type angle)
  2342. {
  2343. T a = DEG2RAD(angle);
  2344. T c = cos(a);
  2345. T s = sin(a);
  2346. this->value[0] = col_type(c, 0,-s, 0);
  2347. this->value[1] = col_type(0, 1, 0, 0);
  2348. this->value[2] = col_type(s, 0, c, 0);
  2349. this->value[3] = col_type(0, 0, 0, 1);
  2350. return *this;
  2351. }
  2352. template<typename U>
  2353. tmat4x4<T>& rotateY(U angle)
  2354. {
  2355. T a = DEG2RAD(angle);
  2356. T c = cos(a);
  2357. T s = sin(a);
  2358. this->value[0] = col_type(c, 0,-s, 0);
  2359. this->value[1] = col_type(0, 1, 0, 0);
  2360. this->value[2] = col_type(s, 0, c, 0);
  2361. this->value[3] = col_type(0, 0, 0, 1);
  2362. return *this;
  2363. }
  2364. tmat4x4<T>& rotateZ(value_type angle)
  2365. {
  2366. T a = T(DEG2RAD(angle));
  2367. T c = cos(a);
  2368. T s = sin(a);
  2369. this->value[0] = col_type( c, s, 0, 0);
  2370. this->value[1] = col_type(-s, c, 0, 0);
  2371. this->value[2] = col_type( 0, 0, 1, 0);
  2372. this->value[3] = col_type( 0, 0, 0, 1);
  2373. return *this;
  2374. }
  2375. template<typename U>
  2376. tmat4x4<T>& rotateZ(U angle)
  2377. {
  2378. T a = DEG2RAD(angle);
  2379. T c = cos(a);
  2380. T s = sin(a);
  2381. this->value[0] = col_type( c, s, 0, 0);
  2382. this->value[1] = col_type(-s, c, 0, 0);
  2383. this->value[2] = col_type( 0, 0, 1, 0);
  2384. this->value[3] = col_type( 0, 0, 0, 1);
  2385. return *this;
  2386. }
  2387. tmat4x4<T> rotateXY(T angleX, T angleY)
  2388. {
  2389. T cosX = cos(DEG2RAD(angleX));
  2390. T sinX = sin(DEG2RAD(angleX));
  2391. T cosY = cos(DEG2RAD(angleY));
  2392. T sinY = sin(DEG2RAD(angleY));
  2393. this->value[0] = col_type( cosY, -sinX * sinY, cosX * sinY,0);
  2394. this->value[1] = col_type( 0, cosX, sinX, 0);
  2395. this->value[2] = col_type( -sinY , -sinX * cosY, cosX * cosY,0);
  2396. this->value[3] = col_type( 0, 0, 0, 1);
  2397. return *this;
  2398. }
  2399. tmat4x4<T> rotateYX(T angleX, T angleY)
  2400. {
  2401. T cosX = cos(DEG2RAD(angleX));
  2402. T sinX = sin(DEG2RAD(angleX));
  2403. T cosY = cos(DEG2RAD(angleY));
  2404. T sinY = sin(DEG2RAD(angleY));
  2405. this->value[0] = col_type( cosY, 0, sinY, 0);
  2406. this->value[1] = col_type( -sinX * sinY,cosX, sinX * cosY,0);
  2407. this->value[2] = col_type( -cosX * sinY,-sinX, cosX * cosY,0);
  2408. this->value[3] = col_type( 0, 0, 0, 1);
  2409. return *this;
  2410. }
  2411. tmat4x4<T> rotateYXZ( T yaw, T pitch, T roll)
  2412. {
  2413. T tmp_ch = cos(DEG2RAD(yaw));
  2414. T tmp_sh = sin(DEG2RAD(yaw));
  2415. T tmp_cp = cos(DEG2RAD(pitch));
  2416. T tmp_sp = sin(DEG2RAD(pitch));
  2417. T tmp_cb = cos(DEG2RAD(roll));
  2418. T tmp_sb = sin(DEG2RAD(roll));
  2419. tmat4x4<T> Result;
  2420. this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb;
  2421. this->value[0][1] = tmp_sb * tmp_cp;
  2422. this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb;
  2423. this->value[0][3] = T(0);
  2424. this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb;
  2425. this->value[1][1] = tmp_cb * tmp_cp;
  2426. this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb;
  2427. this->value[1][3] = T(0);
  2428. this->value[2][0] = tmp_sh * tmp_cp;
  2429. this->value[2][1] = -tmp_sp;
  2430. this->value[2][2] = tmp_ch * tmp_cp;
  2431. this->value[2][3] = T(0);
  2432. this->value[3][0] = T(0);
  2433. this->value[3][1] = T(0);
  2434. this->value[3][2] = T(0);
  2435. this->value[3][3] = T(1);
  2436. return *this;
  2437. }
  2438. tmat4x4<T> yawPitchRoll( T yaw, T pitch, T roll)
  2439. {
  2440. T tmp_ch = cos(DEG2RAD(yaw));
  2441. T tmp_sh = sin(DEG2RAD(yaw));
  2442. T tmp_cp = cos(DEG2RAD(pitch));
  2443. T tmp_sp = sin(DEG2RAD(pitch));
  2444. T tmp_cb = cos(DEG2RAD(roll));
  2445. T tmp_sb = sin(DEG2RAD(roll));
  2446. this->value[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb;
  2447. this->value[0][1] = tmp_sb * tmp_cp;
  2448. this->value[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb;
  2449. this->value[0][3] = T(0);
  2450. this->value[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb;
  2451. this->value[1][1] = tmp_cb * tmp_cp;
  2452. this->value[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb;
  2453. this->value[1][3] = T(0);
  2454. this->value[2][0] = tmp_sh * tmp_cp;
  2455. this->value[2][1] = -tmp_sp;
  2456. this->value[2][2] = tmp_ch * tmp_cp;
  2457. this->value[2][3] = T(0);
  2458. this->value[3][0] = T(0);
  2459. this->value[3][1] = T(0);
  2460. this->value[3][2] = T(0);
  2461. this->value[3][3] = T(1);
  2462. return *this;
  2463. }
  2464. tmat4x4<T>& scale(tvec3<T> const& s)
  2465. {
  2466. this->value[0] = col_type(s[0], 0, 0, 0);
  2467. this->value[1] = col_type(0, s[1], 0, 0);
  2468. this->value[2] = col_type(0, 0, s[2], 0);
  2469. this->value[3] = col_type(0, 0, 0, 1);
  2470. return *this;
  2471. }
  2472. tmat4x4<T>& scale(value_type x,value_type y,value_type z)
  2473. {
  2474. this->value[0] = col_type(x, 0, 0, 0);
  2475. this->value[1] = col_type(0, y, 0, 0);
  2476. this->value[2] = col_type(0, 0, z, 0);
  2477. this->value[3] = col_type(0, 0, 0, 1);
  2478. return *this;
  2479. }
  2480. template<typename U>
  2481. tmat4x4<T>& scale(U x,U y,U z)
  2482. {
  2483. this->value[0] = col_type(value_type(x), 0, 0, 0);
  2484. this->value[1] = col_type(0, value_type(y), 0, 0);
  2485. this->value[2] = col_type(0, 0, value_type(z), 0);
  2486. this->value[3] = col_type(0, 0, 0, 1);
  2487. return *this;
  2488. }
  2489. template<typename U,typename V,typename W>
  2490. tmat4x4<T>& scale(U x,V y,W z)
  2491. {
  2492. this->value[0] = col_type(value_type(x), 0, 0, 0);
  2493. this->value[1] = col_type(0, value_type(y), 0, 0);
  2494. this->value[2] = col_type(0, 0, value_type(z), 0);
  2495. this->value[3] = col_type(0, 0, 0, 1);
  2496. return *this;
  2497. }
  2498. tmat4x4<T> transpose( ) const
  2499. {
  2500. return tmat4x4<T>(
  2501. this->value[0][0], this->value[1][0], this->value[2][0], this->value[3][0],
  2502. this->value[0][1], this->value[1][1], this->value[2][1], this->value[3][1],
  2503. this->value[0][2], this->value[1][2], this->value[2][2], this->value[3][2],
  2504. this->value[0][3], this->value[1][3], this->value[2][3], this->value[3][3]
  2505. );
  2506. }
  2507. tmat4x4<T> extractMatrixRotation() const
  2508. {
  2509. return tmat4x4<T>(
  2510. this->value[0][0], this->value[0][1], this->value[0][2], 0.0,
  2511. this->value[1][0], this->value[1][1], this->value[1][2], 0.0,
  2512. this->value[2][0], this->value[2][1], this->value[2][2], 0.0,
  2513. 0.0, 0.0, 0.0, 1.0
  2514. );
  2515. }
  2516. };
  2517. template <typename T>
  2518. tmat4x4<T> rotateX(T angleX)
  2519. {
  2520. T cosX = cos(DEG2RAD(angleX));
  2521. T sinX = sin(DEG2RAD(angleX));
  2522. return tmat4x4<T>(
  2523. T(1), T(0), T(0), T(0),
  2524. T(0), cosX, sinX, T(0),
  2525. T(0),-sinX, cosX, T(0),
  2526. T(0), T(0), T(0), T(1));
  2527. }
  2528. template <typename T>
  2529. tmat4x4<T> rotateY(T angleY)
  2530. {
  2531. T cosY = cos(DEG2RAD(angleY));
  2532. T sinY = sin(DEG2RAD(angleY));
  2533. return tmat4x4<T>(
  2534. cosY, T(0), sinY, T(0),
  2535. T(0), T(1), T(0), T(0),
  2536. -sinY, T(0), cosY, T(0),
  2537. T(0), T(0), T(0), T(1));
  2538. }
  2539. template <typename T>
  2540. tmat4x4<T> rotateZ(T angleZ)
  2541. {
  2542. T cosZ = (T)cos(DEG2RAD(angleZ));
  2543. T sinZ = (T)sin(DEG2RAD(angleZ));
  2544. return tmat4x4<T>(
  2545. cosZ, sinZ, T(0), T(0),
  2546. -sinZ, cosZ, T(0), T(0),
  2547. T(0), T(0), T(1), T(0),
  2548. T(0), T(0), T(0), T(1));
  2549. }
  2550. template <typename T>
  2551. tmat4x4<T> rotateXY(T angleX, T angleY)
  2552. {
  2553. T cosX = cos(DEG2RAD(angleX));
  2554. T sinX = sin(DEG2RAD(angleX));
  2555. T cosY = cos(DEG2RAD(angleY));
  2556. T sinY = sin(DEG2RAD(angleY));
  2557. return tmat4x4<T>(
  2558. cosY, -sinX * sinY, cosX * sinY, T(0),
  2559. T(0), cosX, sinX, T(0),
  2560. -sinY, -sinX * cosY, cosX * cosY, T(0),
  2561. T(0), T(0), T(0), T(1));
  2562. }
  2563. template <typename T>
  2564. tmat4x4<T> rotateYX(T angleY, T angleX)
  2565. {
  2566. T cosX = cos(DEG2RAD(angleX));
  2567. T sinX = sin(DEG2RAD(angleX));
  2568. T cosY = cos(DEG2RAD(angleY));
  2569. T sinY = sin(DEG2RAD(angleY));
  2570. return tmat4x4<T>(
  2571. cosY, T(0), sinY, T(0),
  2572. -sinX * sinY, cosX, sinX * cosY, T(0),
  2573. -cosX * sinY, -sinX, cosX * cosY, T(0),
  2574. T(0), T(0), T(0), T(1));
  2575. }
  2576. template <typename T>
  2577. tmat4x4<T> rotateXZ(T angleX, T angleZ)
  2578. {
  2579. return rotateX(angleX) * rotateZ(angleZ);
  2580. }
  2581. template <typename T>
  2582. tmat4x4<T> rotateZX(T angleX, T angleZ)
  2583. {
  2584. return rotateZ(angleZ) * rotateX(angleX);
  2585. }
  2586. template <typename T>
  2587. tmat4x4<T> rotateYXZ(T yaw, T pitch, T roll)
  2588. {
  2589. T tmp_ch = cos(DEG2RAD(yaw));
  2590. T tmp_sh = sin(DEG2RAD(yaw));
  2591. T tmp_cp = cos(DEG2RAD(pitch));
  2592. T tmp_sp = sin(DEG2RAD(pitch));
  2593. T tmp_cb = cos(DEG2RAD(roll));
  2594. T tmp_sb = sin(DEG2RAD(roll));
  2595. tmat4x4<T> res;
  2596. res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb;
  2597. res[0][1] = tmp_sb * tmp_cp;
  2598. res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb;
  2599. res[0][3] = T(0);
  2600. res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb;
  2601. res[1][1] = tmp_cb * tmp_cp;
  2602. res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb;
  2603. res[1][3] = T(0);
  2604. res[2][0] = tmp_sh * tmp_cp;
  2605. res[2][1] = -tmp_sp;
  2606. res[2][2] = tmp_ch * tmp_cp;
  2607. res[2][3] = T(0);
  2608. res[3][0] = T(0);
  2609. res[3][1] = T(0);
  2610. res[3][2] = T(0);
  2611. res[3][3] = T(1);
  2612. return res;
  2613. }
  2614. template <typename T>
  2615. tmat4x4<T> yawPitchRoll(T yaw, T pitch, T roll)
  2616. {
  2617. T tmp_ch = cos(DEG2RAD(yaw));
  2618. T tmp_sh = sin(DEG2RAD(yaw));
  2619. T tmp_cp = cos(DEG2RAD(pitch));
  2620. T tmp_sp = sin(DEG2RAD(pitch));
  2621. T tmp_cb = cos(DEG2RAD(roll));
  2622. T tmp_sb = sin(DEG2RAD(roll));
  2623. tmat4x4<T> res;
  2624. res[0][0] = tmp_ch * tmp_cb + tmp_sh * tmp_sp * tmp_sb;
  2625. res[0][1] = tmp_sb * tmp_cp;
  2626. res[0][2] = -tmp_sh * tmp_cb + tmp_ch * tmp_sp * tmp_sb;
  2627. res[0][3] = T(0);
  2628. res[1][0] = -tmp_ch * tmp_sb + tmp_sh * tmp_sp * tmp_cb;
  2629. res[1][1] = tmp_cb * tmp_cp;
  2630. res[1][2] = tmp_sb * tmp_sh + tmp_ch * tmp_sp * tmp_cb;
  2631. res[1][3] = T(0);
  2632. res[2][0] = tmp_sh * tmp_cp;
  2633. res[2][1] = -tmp_sp;
  2634. res[2][2] = tmp_ch * tmp_cp;
  2635. res[2][3] = T(0);
  2636. res[3][0] = T(0);
  2637. res[3][1] = T(0);
  2638. res[3][2] = T(0);
  2639. res[3][3] = T(1);
  2640. return res;
  2641. }
  2642. template <typename T>
  2643. void axisAngle
  2644. (
  2645. tmat4x4<T> const & mat,
  2646. tvec3<T> & axis,
  2647. T & angle
  2648. )
  2649. {
  2650. T epsilon = (T)0.01;
  2651. T epsilon2 = (T)0.1;
  2652. if ((fabs(mat[1][0] - mat[0][1]) < epsilon) &&
  2653. (fabs(mat[2][0] - mat[0][2]) < epsilon) &&
  2654. (fabs(mat[2][1] - mat[1][2]) < epsilon))
  2655. {
  2656. if ((fabs(mat[1][0] + mat[0][1]) < epsilon2) &&
  2657. (fabs(mat[2][0] + mat[0][2]) < epsilon2) &&
  2658. (fabs(mat[2][1] + mat[1][2]) < epsilon2) &&
  2659. (fabs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2))
  2660. {
  2661. angle = (T)0.0;
  2662. axis.x = (T)1.0;
  2663. axis.y = (T)0.0;
  2664. axis.z = (T)0.0;
  2665. return;
  2666. }
  2667. angle = T(3.1415926535897932384626433832795);
  2668. T xx = (mat[0][0] + (T)1.0) / (T)2.0;
  2669. T yy = (mat[1][1] + (T)1.0) / (T)2.0;
  2670. T zz = (mat[2][2] + (T)1.0) / (T)2.0;
  2671. T xy = (mat[1][0] + mat[0][1]) / (T)4.0;
  2672. T xz = (mat[2][0] + mat[0][2]) / (T)4.0;
  2673. T yz = (mat[2][1] + mat[1][2]) / (T)4.0;
  2674. if ((xx > yy) && (xx > zz))
  2675. {
  2676. if (xx < epsilon)
  2677. {
  2678. axis.x = (T)0.0;
  2679. axis.y = (T)0.7071;
  2680. axis.z = (T)0.7071;
  2681. }
  2682. else
  2683. {
  2684. axis.x = sqrt(xx);
  2685. axis.y = xy / axis.x;
  2686. axis.z = xz / axis.x;
  2687. }
  2688. }
  2689. else if (yy > zz)
  2690. {
  2691. if (yy < epsilon)
  2692. {
  2693. axis.x = (T)0.7071;
  2694. axis.y = (T)0.0;
  2695. axis.z = (T)0.7071;
  2696. }
  2697. else
  2698. {
  2699. axis.y = sqrt(yy);
  2700. axis.x = xy / axis.y;
  2701. axis.z = yz / axis.y;
  2702. }
  2703. }
  2704. else
  2705. {
  2706. if (zz < epsilon)
  2707. {
  2708. axis.x = (T)0.7071;
  2709. axis.y = (T)0.7071;
  2710. axis.z = (T)0.0;
  2711. }
  2712. else
  2713. {
  2714. axis.z = sqrt(zz);
  2715. axis.x = xz / axis.z;
  2716. axis.y = yz / axis.z;
  2717. }
  2718. }
  2719. return;
  2720. }
  2721. T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1]));
  2722. if (abs(s) < T(0.001))
  2723. s = (T)1.0;
  2724. angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0);
  2725. axis.x = (mat[1][2] - mat[2][1]) / s;
  2726. axis.y = (mat[2][0] - mat[0][2]) / s;
  2727. axis.z = (mat[0][1] - mat[1][0]) / s;
  2728. }
  2729. template <typename T>
  2730. tmat4x4<T> axisAngleMatrix(tvec3<T> const & axis,T const angle)
  2731. {
  2732. T c = cos(angle);
  2733. T s = sin(angle);
  2734. T t = T(1) - c;
  2735. tvec3<T> n = normalize(axis);
  2736. return tmat4x4<T>(
  2737. t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0),
  2738. t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0),
  2739. t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0),
  2740. T(0), T(0), T(0), T(1)
  2741. );
  2742. }
  2743. template <typename T>
  2744. tmat4x4<T> interpolate
  2745. (
  2746. tmat4x4<T> const & m1,
  2747. tmat4x4<T> const & m2,
  2748. T const delta
  2749. )
  2750. {
  2751. tmat4x4<T> m1rot = m1.extractMatrixRotation();
  2752. tmat4x4<T> dltRotation = m2 * m1rot.transpose();
  2753. tvec3<T> dltAxis;
  2754. T dltAngle;
  2755. axisAngle(dltRotation, dltAxis, dltAngle);
  2756. tmat4x4<T> out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot;
  2757. out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]);
  2758. out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]);
  2759. out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]);
  2760. return out;
  2761. }
  2762. template <typename T>
  2763. tvec3<T> operator * (tvec3<T> const& v, tmat4x4<T> const& mat)
  2764. {
  2765. return tvec3<T>
  2766. (
  2767. v.x*mat[0][0] + v.y*mat[1][0] + v.z*mat[2][0] + 1*mat[3][0],
  2768. v.x*mat[0][1] + v.y*mat[1][1] + v.z*mat[2][1] + 1*mat[3][1],
  2769. v.x*mat[0][2] + v.y*mat[1][2] + v.z*mat[2][2] + 1*mat[3][2]
  2770. );
  2771. }
  2772. template <typename T>
  2773. tmat4x4<T> operator+ (tmat4x4<T> const & m, typename tmat4x4<T>::value_type s)
  2774. {
  2775. return tmat4x4<T>(
  2776. m[0] + s,
  2777. m[1] + s,
  2778. m[2] + s,
  2779. m[3] + s);
  2780. }
  2781. template <typename T>
  2782. tmat4x4<T> operator+ (typename tmat4x4<T>::value_type s, tmat4x4<T> const & m)
  2783. {
  2784. return tmat4x4<T>(
  2785. m[0] + s,
  2786. m[1] + s,
  2787. m[2] + s,
  2788. m[3] + s);
  2789. }
  2790. template <typename T>
  2791. tmat4x4<T> operator+ (tmat4x4<T> const & m1, tmat4x4<T> const & m2)
  2792. {
  2793. return tmat4x4<T>(
  2794. m1[0] + m2[0],
  2795. m1[1] + m2[1],
  2796. m1[2] + m2[2],
  2797. m1[3] + m2[3]);
  2798. }
  2799. template <typename T>
  2800. tmat4x4<T> operator- (tmat4x4<T> const & m, typename tmat4x4<T>::value_type s)
  2801. {
  2802. return tmat4x4<T>(
  2803. m[0] - s,
  2804. m[1] - s,
  2805. m[2] - s,
  2806. m[3] - s);
  2807. }
  2808. template <typename T>
  2809. tmat4x4<T> operator- (typename tmat4x4<T>::value_type s, tmat4x4<T> const & m)
  2810. {
  2811. return tmat4x4<T>(
  2812. s - m[0],
  2813. s - m[1],
  2814. s - m[2],
  2815. s - m[3]);
  2816. }
  2817. template <typename T>
  2818. tmat4x4<T> operator- (tmat4x4<T> const & m1, tmat4x4<T> const & m2)
  2819. {
  2820. return tmat4x4<T>(
  2821. m1[0] - m2[0],
  2822. m1[1] - m2[1],
  2823. m1[2] - m2[2],
  2824. m1[3] - m2[3]);
  2825. }
  2826. template <typename T>
  2827. tmat4x4<T> operator* (tmat4x4<T> const & m, typename tmat4x4<T>::value_type s)
  2828. {
  2829. return tmat4x4<T>(
  2830. m[0] * s,
  2831. m[1] * s,
  2832. m[2] * s,
  2833. m[3] * s);
  2834. }
  2835. template <typename T>
  2836. tmat4x4<T> operator* (typename tmat4x4<T>::value_type s, tmat4x4<T> const & m)
  2837. {
  2838. return tmat4x4<T>(
  2839. m[0] * s,
  2840. m[1] * s,
  2841. m[2] * s,
  2842. m[3] * s);
  2843. }
  2844. template <typename T>
  2845. typename tmat4x4<T>::col_type operator* (tmat4x4<T> const & m, typename tmat4x4<T>::row_type const & v)
  2846. {
  2847. return typename tmat4x4<T>::col_type(
  2848. m[0][0] * v.x + m[1][0] * v.y + m[2][0] * v.z + m[3][0] * v.w,
  2849. m[0][1] * v.x + m[1][1] * v.y + m[2][1] * v.z + m[3][1] * v.w,
  2850. m[0][2] * v.x + m[1][2] * v.y + m[2][2] * v.z + m[3][2] * v.w,
  2851. m[0][3] * v.x + m[1][3] * v.y + m[2][3] * v.z + m[3][3] * v.w);
  2852. }
  2853. template <typename T>
  2854. typename tmat4x4<T>::row_type operator* (typename tmat4x4<T>::col_type const & v, tmat4x4<T> const & m)
  2855. {
  2856. return typename tmat4x4<T>::row_type(
  2857. m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w,
  2858. m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w,
  2859. m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w,
  2860. m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w);
  2861. }
  2862. template <typename T>
  2863. tmat4x4<T> operator* (tmat4x4<T> const & m1, tmat4x4<T> const & m2)
  2864. {
  2865. typename tmat4x4<T>::col_type const srcA0 = m1[0];
  2866. typename tmat4x4<T>::col_type const srcA1 = m1[1];
  2867. typename tmat4x4<T>::col_type const srcA2 = m1[2];
  2868. typename tmat4x4<T>::col_type const srcA3 = m1[3];
  2869. typename tmat4x4<T>::col_type const srcB0 = m2[0];
  2870. typename tmat4x4<T>::col_type const srcB1 = m2[1];
  2871. typename tmat4x4<T>::col_type const srcB2 = m2[2];
  2872. typename tmat4x4<T>::col_type const srcB3 = m2[3];
  2873. tmat4x4<T> res;
  2874. res[0] = srcA0 * srcB0[0] + srcA1 * srcB0[1] + srcA2 * srcB0[2] + srcA3 * srcB0[3];
  2875. res[1] = srcA0 * srcB1[0] + srcA1 * srcB1[1] + srcA2 * srcB1[2] + srcA3 * srcB1[3];
  2876. res[2] = srcA0 * srcB2[0] + srcA1 * srcB2[1] + srcA2 * srcB2[2] + srcA3 * srcB2[3];
  2877. res[3] = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3];
  2878. return res;
  2879. }
  2880. template <typename T>
  2881. tmat4x4<T> operator/ (tmat4x4<T> const & m, typename tmat4x4<T>::value_type s)
  2882. {
  2883. return tmat4x4<T>(
  2884. m[0] / s,
  2885. m[1] / s,
  2886. m[2] / s,
  2887. m[3] / s);
  2888. }
  2889. template <typename T>
  2890. tmat4x4<T> operator/ (typename tmat4x4<T>::value_type s, tmat4x4<T> const & m)
  2891. {
  2892. return tmat4x4<T>(
  2893. s / m[0],
  2894. s / m[1],
  2895. s / m[2],
  2896. s / m[3]);
  2897. }
  2898. template <typename T>
  2899. typename tmat4x4<T>::col_type operator/ (tmat4x4<T> const & m, typename tmat4x4<T>::row_type const & v)
  2900. {
  2901. return m.inverse() * v;
  2902. }
  2903. template <typename T>
  2904. typename tmat4x4<T>::row_type operator/ (typename tmat4x4<T>::col_type const & v, tmat4x4<T> const & m)
  2905. {
  2906. return v * m.inverse();
  2907. }
  2908. template <typename T>
  2909. tmat4x4<T> operator/ (tmat4x4<T> const & m1, tmat4x4<T> const & m2)
  2910. {
  2911. return m1 * m2.inverse();
  2912. }
  2913. template <typename T>
  2914. tmat4x4<T> const operator- (tmat4x4<T> const & m)
  2915. {
  2916. return tmat4x4<T>(
  2917. -m[0],
  2918. -m[1],
  2919. -m[2],
  2920. -m[3]);
  2921. }
  2922. template <typename T>
  2923. tmat4x4<T> const operator++ (tmat4x4<T> const & m, int)
  2924. {
  2925. return tmat4x4<T>(
  2926. m[0] + T(1),
  2927. m[1] + T(1),
  2928. m[2] + T(1),
  2929. m[3] + T(1));
  2930. }
  2931. template <typename T>
  2932. tmat4x4<T> const operator-- (tmat4x4<T> const & m, int)
  2933. {
  2934. return tmat4x4<T>(
  2935. m[0] - T(1),
  2936. m[1] - T(1),
  2937. m[2] - T(1),
  2938. m[3] - T(1));
  2939. }
  2940. template <typename T>
  2941. bool operator==(tmat4x4<T> const & m1, tmat4x4<T> const & m2)
  2942. {
  2943. return (m1[0] == m2[0]) && (m1[1] == m2[1]) && (m1[2] == m2[2]) && (m1[3] == m2[3]);
  2944. }
  2945. template <typename T>
  2946. bool operator!=(tmat4x4<T> const & m1, tmat4x4<T> const & m2)
  2947. {
  2948. return (m1[0] != m2[0]) || (m1[1] != m2[1]) || (m1[2] != m2[2]) || (m1[3] != m2[3]);
  2949. }
  2950. //////////////////////////////////////////////////////////////////////////
  2951. //////////////////////////////////////////////////////////////////////////
  2952. //! ������
  2953. //////////////////////////////////////////////////////////////////////////
  2954. //////////////////////////////////////////////////////////////////////////
  2955. //////////////////////////////////////////////////////////////////////////////////////////////////////////
  2956. template <typename T>
  2957. typename tvec2<T>::value_type length(tvec2<T> const & v)
  2958. {
  2959. typename tvec2<T>::value_type sqr = v.x * v.x + v.y * v.y;
  2960. return sqrt(sqr);
  2961. }
  2962. template <typename T>
  2963. typename tvec3<T>::value_type length(tvec3<T> const & v)
  2964. {
  2965. typename tvec3<T>::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z;
  2966. return sqrt(sqr);
  2967. }
  2968. template <typename T>
  2969. typename tvec4<T>::value_type length(tvec4<T> const & v)
  2970. {
  2971. typename tvec4<T>::value_type sqr = v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w;
  2972. return sqrt(sqr);
  2973. }
  2974. template <typename T>
  2975. typename tvec2<T>::value_type distance(tvec2<T> const & p0,tvec2<T> const & p1)
  2976. {
  2977. return length(p1 - p0);
  2978. }
  2979. template <typename T>
  2980. typename tvec3<T>::value_type distance(tvec3<T> const & p0,tvec3<T> const & p1)
  2981. {
  2982. return length(p1 - p0);
  2983. }
  2984. template <typename T>
  2985. typename tvec4<T>::value_type distance(tvec4<T> const & p0,tvec4<T> const & p1)
  2986. {
  2987. return length(p1 - p0);
  2988. }
  2989. template <typename T>
  2990. typename tvec2<T>::value_type dot(tvec2<T> const & x, tvec2<T> const & y)
  2991. {
  2992. return x.x * y.x + x.y * y.y;
  2993. }
  2994. template <typename T>
  2995. typename tvec3<T>::value_type dot(tvec3<T> const & x, tvec3<T> const & y)
  2996. {
  2997. return x.x * y.x + x.y * y.y + x.z * y.z;
  2998. }
  2999. template <typename T>
  3000. typename tvec4<T>::value_type dot(tvec4<T> const & x, tvec4<T> const & y)
  3001. {
  3002. return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w;
  3003. }
  3004. template <typename T>
  3005. tvec3<T> cross(tvec3<T> const & x, tvec3<T> const & y)
  3006. {
  3007. return tvec3<T>
  3008. (
  3009. x.y * y.z - y.y * x.z,
  3010. x.z * y.x - y.z * x.x,
  3011. x.x * y.y - y.x * x.y
  3012. );
  3013. }
  3014. template <typename T>
  3015. T inversesqrt(T x)
  3016. {
  3017. return T(1) / sqrt(x);
  3018. }
  3019. template <typename T>
  3020. tvec2<T> normalize(tvec2<T> const & x)
  3021. {
  3022. typename tvec2<T>::value_type sqr = x.x * x.x + x.y * x.y;
  3023. return x * inversesqrt(sqr);
  3024. }
  3025. template <typename T>
  3026. tvec3<T> normalize(tvec3<T> const & x)
  3027. {
  3028. typename tvec3<T>::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z;
  3029. return x * inversesqrt(sqr);
  3030. }
  3031. template <typename T>
  3032. tvec4<T> normalize(tvec4<T> const & x)
  3033. {
  3034. typename tvec4<T>::value_type sqr = x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w;
  3035. return x * inversesqrt(sqr);
  3036. }
  3037. //////////////////////////////////////////////////////////////////////////
  3038. //////////////////////////////////////////////////////////////////////////
  3039. //////////////////////////////////////////////////////////////////////////
  3040. template <typename T>
  3041. struct tquat
  3042. {
  3043. typedef T value_type;
  3044. typedef std::size_t size_type;
  3045. public:
  3046. value_type x;
  3047. value_type y;
  3048. value_type z;
  3049. value_type w;
  3050. size_type length() const
  3051. {
  3052. return 4;
  3053. }
  3054. tquat():
  3055. x(0),
  3056. y(0),
  3057. z(0),
  3058. w(1)
  3059. {}
  3060. explicit tquat(value_type s, tvec3<T> const & v):
  3061. x(v.x),
  3062. y(v.y),
  3063. z(v.z),
  3064. w(s)
  3065. {
  3066. }
  3067. explicit tquat(tvec3<T> const & v,value_type s):
  3068. x(v.x),
  3069. y(v.y),
  3070. z(v.z),
  3071. w(s)
  3072. {
  3073. }
  3074. explicit tquat(value_type w, value_type x, value_type y, value_type z):
  3075. x(x),
  3076. y(y),
  3077. z(z),
  3078. w(w)
  3079. {}
  3080. explicit tquat(tvec3<T> const & eulerAngle)
  3081. {
  3082. tvec3<T> c = cos(eulerAngle * value_type(0.5));
  3083. tvec3<T> s = sin(eulerAngle * value_type(0.5));
  3084. this->w = c.x * c.y * c.z + s.x * s.y * s.z;
  3085. this->x = s.x * c.y * c.z - c.x * s.y * s.z;
  3086. this->y = c.x * s.y * c.z + s.x * c.y * s.z;
  3087. this->z = c.x * c.y * s.z - s.x * s.y * c.z;
  3088. }
  3089. explicit tquat(tmat3x3<T> const & m)
  3090. {
  3091. *this = quat_cast(m);
  3092. }
  3093. explicit tquat(tmat4x4<T> const & m)
  3094. {
  3095. *this = quat_cast(m);
  3096. }
  3097. value_type & operator[](int i)
  3098. {
  3099. return (&x)[i];
  3100. }
  3101. value_type const & operator[](int i) const
  3102. {
  3103. return (&x)[i];
  3104. }
  3105. tquat<T> & operator*=(value_type s)
  3106. {
  3107. this->w *= s;
  3108. this->x *= s;
  3109. this->y *= s;
  3110. this->z *= s;
  3111. return *this;
  3112. }
  3113. tquat<T> & operator = (const tquat<T>& right)
  3114. {
  3115. this->w = right.w;
  3116. this->x = right.x;
  3117. this->y = right.y;
  3118. this->z = right.z;
  3119. return *this;
  3120. }
  3121. tquat<T> & operator/=(value_type s)
  3122. {
  3123. this->w /= s;
  3124. this->x /= s;
  3125. this->y /= s;
  3126. this->z /= s;
  3127. return *this;
  3128. }
  3129. };
  3130. template< typename T>
  3131. tmat4x4<T> makeTransform( tvec3<T> const & position, tvec3<T> const& scale, const tquat<T>& orientation)
  3132. {
  3133. tmat3x3<T> rot3x3 = mat3_cast(orientation);
  3134. return tmat4x4<T>
  3135. (
  3136. scale.x * rot3x3[0][0], scale.x * rot3x3[0][1], scale.x * rot3x3[0][2], 0,
  3137. scale.y * rot3x3[1][0], scale.y * rot3x3[1][1], scale.y * rot3x3[1][2], 0,
  3138. scale.z * rot3x3[2][0], scale.z * rot3x3[2][1], scale.z * rot3x3[2][2], 0,
  3139. position.x, position.y, position.z, 1
  3140. );
  3141. }
  3142. template <typename T>
  3143. T dot(tquat<T> const & q1, tquat<T> const & q2)
  3144. {
  3145. return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
  3146. }
  3147. template <typename T>
  3148. tquat<T> cross(tquat<T> const & q1, tquat<T> const & q2)
  3149. {
  3150. return tquat<T>(
  3151. q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
  3152. q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
  3153. q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z,
  3154. q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x);
  3155. }
  3156. template <typename T>
  3157. T length(tquat<T> const & q)
  3158. {
  3159. return sqrt(dot(q, q));
  3160. }
  3161. template <typename genType>
  3162. genType mix(genType x, genType y, genType a)
  3163. {
  3164. return x + a * (y - x);
  3165. }
  3166. template <typename T>
  3167. T epsilon()
  3168. {
  3169. return std::numeric_limits<T>::epsilon();
  3170. }
  3171. template <typename T>
  3172. tquat<T> conjugate(tquat<T> const & q)
  3173. {
  3174. return tquat<T>(q.w, -q.x, -q.y, -q.z);
  3175. }
  3176. template <typename T>
  3177. tquat<T> inverse(tquat<T> const & q)
  3178. {
  3179. return conjugate(q) / dot(q, q);
  3180. }
  3181. template <typename T>
  3182. bool operator==(tquat<T> const & q1, tquat<T> const & q2)
  3183. {
  3184. return (q1.x == q2.x) && (q1.y == q2.y) && (q1.z == q2.z) && (q1.w == q2.w);
  3185. }
  3186. template <typename T>
  3187. bool operator!=(tquat<T> const & q1, tquat<T> const & q2)
  3188. {
  3189. return (q1.x != q2.x) || (q1.y != q2.y) || (q1.z != q2.z) || (q1.w != q2.w);
  3190. }
  3191. template <typename T>
  3192. tquat<T> operator- (tquat<T> const & q)
  3193. {
  3194. return tquat<T>(-q.w, -q.x, -q.y, -q.z);
  3195. }
  3196. template <typename T>
  3197. tquat<T> operator+ ( tquat<T> const & q, tquat<T> const & p)
  3198. {
  3199. return tquat<T>(
  3200. q.w + p.w,
  3201. q.x + p.x,
  3202. q.y + p.y,
  3203. q.z + p.z
  3204. );
  3205. }
  3206. template <typename T>
  3207. tquat<T> operator* ( tquat<T> const & q, tquat<T> const & p)
  3208. {
  3209. return tquat<T>(
  3210. q.w * p.w - q.x * p.x - q.y * p.y - q.z * p.z,
  3211. q.w * p.x + q.x * p.w + q.y * p.z - q.z * p.y,
  3212. q.w * p.y + q.y * p.w + q.z * p.x - q.x * p.z,
  3213. q.w * p.z + q.z * p.w + q.x * p.y - q.y * p.x
  3214. );
  3215. }
  3216. template <typename T>
  3217. tvec3<T> operator* (tquat<T> const & q, tvec3<T> const & v)
  3218. {
  3219. typename tquat<T>::value_type two(2);
  3220. tvec3<T> uv;
  3221. tvec3<T> uuv;
  3222. tvec3<T> quatVector(q.x, q.y, q.z);
  3223. uv = cross(quatVector, v);
  3224. uuv = cross(quatVector, uv);
  3225. uv *= two * q.w;
  3226. uuv *= two;
  3227. return v + uv + uuv;
  3228. }
  3229. template <typename T>
  3230. tvec3<T> operator* (tvec3<T> const & v,tquat<T> const & q)
  3231. {
  3232. return inverse(q) * v;
  3233. }
  3234. template <typename T>
  3235. tquat<T> operator* (tquat<T> const & q, typename tquat<T>::value_type s)
  3236. {
  3237. return tquat<T>(q.w * s, q.x * s, q.y * s, q.z * s);
  3238. }
  3239. template <typename T>
  3240. tquat<T> operator* (typename tquat<T>::value_type s,tquat<T> const & q)
  3241. {
  3242. return q * s;
  3243. }
  3244. template <typename T>
  3245. tquat<T> operator/ (tquat<T> const & q, typename tquat<T>::value_type s)
  3246. {
  3247. return tquat<T>(q.w / s, q.x / s, q.y / s, q.z / s);
  3248. }
  3249. template <typename T>
  3250. tquat<T> mix(tquat<T> const & x, tquat<T> const & y, T const & a)
  3251. {
  3252. T cosTheta = dot(x, y);
  3253. if(cosTheta > T(1) - epsilon<T>())
  3254. {
  3255. return tquat<T>(
  3256. mix(x.w, y.w, a),
  3257. mix(x.x, y.x, a),
  3258. mix(x.y, y.y, a),
  3259. mix(x.z, y.z, a)
  3260. );
  3261. }
  3262. else
  3263. {
  3264. // Essential Mathematics, page 467
  3265. T angle = acos(cosTheta);
  3266. return (sin((T(1) - a) * angle) * x + sin(a * angle) * y) / sin(angle);
  3267. }
  3268. }
  3269. template <typename T>
  3270. tquat<T> lerp(tquat<T> const & x, tquat<T> const & y, T a)
  3271. {
  3272. assert(a >= T(0));
  3273. assert(a <= T(1));
  3274. return x * (T(1) - a) + (y * a);
  3275. }
  3276. template <typename T>
  3277. tquat<T> slerp(tquat<T> const & x, tquat<T> const & y, T a)
  3278. {
  3279. tquat<T> z = y;
  3280. T cosTheta = dot(x, y);
  3281. if (cosTheta < T(0))
  3282. {
  3283. z = -y;
  3284. cosTheta = -cosTheta;
  3285. }
  3286. if(cosTheta > T(1) - epsilon<T>())
  3287. {
  3288. return tquat<T>
  3289. (
  3290. mix(x.w, z.w, a),
  3291. mix(x.x, z.x, a),
  3292. mix(x.y, z.y, a),
  3293. mix(x.z, z.z, a)
  3294. );
  3295. }
  3296. else
  3297. {
  3298. // Essential Mathematics, page 467
  3299. T angle = acos(cosTheta);
  3300. return (sin((T(1) - a) * angle) * x + sin(a * angle) * z) / sin(angle);
  3301. }
  3302. }
  3303. template <typename T>
  3304. tquat<T> rotate
  3305. (
  3306. typename tquat<T>::value_type angle,
  3307. tvec3<T> const & axis
  3308. )
  3309. {
  3310. tvec3<T> Tmp = axis;
  3311. typename tquat<T>::value_type len = length(Tmp);
  3312. if(abs(len - T(1)) > T(0.001f))
  3313. {
  3314. T oneOverLen = T(1) / len;
  3315. Tmp.x *= oneOverLen;
  3316. Tmp.y *= oneOverLen;
  3317. Tmp.z *= oneOverLen;
  3318. }
  3319. typename tquat<T>::value_type const AngleRad = (T)DEG2RAD(angle);
  3320. typename tquat<T>::value_type const Sin = (T)sin(AngleRad * T(0.5));
  3321. return tquat<T>((T)cos(AngleRad * T(0.5)), Tmp.x * Sin, Tmp.y * Sin, Tmp.z * Sin);
  3322. }
  3323. template <typename valType>
  3324. valType roll(tquat<valType> const & q)
  3325. {
  3326. return atan2(valType(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z) * valType(RAD2DEG);
  3327. }
  3328. template <typename valType>
  3329. valType pitch(tquat<valType> const & q)
  3330. {
  3331. return ::atan2(valType(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* valType(RAD2DEG);
  3332. }
  3333. template <typename valType>
  3334. valType yaw(tquat<valType> const & q)
  3335. {
  3336. return ::asin(valType(-2) * (q.x * q.z - q.w * q.y)) * valType(RAD2DEG);
  3337. }
  3338. template <typename T>
  3339. tvec3<T> eulerAngles(tquat<T> const & x)
  3340. {
  3341. return tvec3<T>(pitch(x), yaw(x), roll(x));
  3342. }
  3343. template <typename T>
  3344. tmat3x3<T> mat3_cast(const tquat<T>& q)
  3345. {
  3346. return tmat3x3<T>
  3347. (
  3348. 1 - 2 * q.y * q.y - 2 * q.z * q.z, 2 * q.x * q.y + 2 * q.w * q.z, 2 * q.x * q.z - 2 * q.w * q.y,
  3349. 2 * q.x * q.y - 2 * q.w * q.z, 1 - 2 * q.x * q.x - 2 * q.z * q.z, 2 * q.y * q.z + 2 * q.w * q.x,
  3350. 2 * q.x * q.z + 2 * q.w * q.y, 2 * q.y * q.z - 2 * q.w * q.x, 1 - 2 * q.x * q.x - 2 * q.y * q.y
  3351. );
  3352. }
  3353. template <typename T>
  3354. tmat4x4<T> mat4_cast(tquat<T> const & q)
  3355. {
  3356. return tmat4x4<T>(mat3_cast(q));
  3357. }
  3358. template <typename T>
  3359. tquat<T> quat_cast(tmat3x3<T> const & m)
  3360. {
  3361. typename tquat<T>::value_type fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2];
  3362. typename tquat<T>::value_type fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2];
  3363. typename tquat<T>::value_type fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1];
  3364. typename tquat<T>::value_type fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2];
  3365. int biggestIndex = 0;
  3366. typename tquat<T>::value_type fourBiggestSquaredMinus1 = fourWSquaredMinus1;
  3367. if(fourXSquaredMinus1 > fourBiggestSquaredMinus1)
  3368. {
  3369. fourBiggestSquaredMinus1 = fourXSquaredMinus1;
  3370. biggestIndex = 1;
  3371. }
  3372. if(fourYSquaredMinus1 > fourBiggestSquaredMinus1)
  3373. {
  3374. fourBiggestSquaredMinus1 = fourYSquaredMinus1;
  3375. biggestIndex = 2;
  3376. }
  3377. if(fourZSquaredMinus1 > fourBiggestSquaredMinus1)
  3378. {
  3379. fourBiggestSquaredMinus1 = fourZSquaredMinus1;
  3380. biggestIndex = 3;
  3381. }
  3382. typename tquat<T>::value_type biggestVal = sqrt(fourBiggestSquaredMinus1 + T(1)) * T(0.5);
  3383. typename tquat<T>::value_type mult = T(0.25) / biggestVal;
  3384. tquat<T> res;
  3385. switch(biggestIndex)
  3386. {
  3387. case 0:
  3388. res.w = biggestVal;
  3389. res.x = (m[1][2] - m[2][1]) * mult;
  3390. res.y = (m[2][0] - m[0][2]) * mult;
  3391. res.z = (m[0][1] - m[1][0]) * mult;
  3392. break;
  3393. case 1:
  3394. res.w = (m[1][2] - m[2][1]) * mult;
  3395. res.x = biggestVal;
  3396. res.y = (m[0][1] + m[1][0]) * mult;
  3397. res.z = (m[2][0] + m[0][2]) * mult;
  3398. break;
  3399. case 2:
  3400. res.w = (m[2][0] - m[0][2]) * mult;
  3401. res.x = (m[0][1] + m[1][0]) * mult;
  3402. res.y = biggestVal;
  3403. res.z = (m[1][2] + m[2][1]) * mult;
  3404. break;
  3405. case 3:
  3406. res.w = (m[0][1] - m[1][0]) * mult;
  3407. res.x = (m[2][0] + m[0][2]) * mult;
  3408. res.y = (m[1][2] + m[2][1]) * mult;
  3409. res.z = biggestVal;
  3410. break;
  3411. default:
  3412. assert(false);
  3413. break;
  3414. }
  3415. return res;
  3416. }
  3417. template <typename T>
  3418. tquat<T> quat_cast(tmat4x4<T> const & m4)
  3419. {
  3420. return quat_cast(tmat3x3<T>(m4[0][0],m4[0][1],m4[0][2],
  3421. m4[1][0],m4[1][1],m4[1][2],
  3422. m4[2][0],m4[2][1],m4[2][2]));
  3423. }
  3424. template <typename T>
  3425. T angle(tquat<T> const & x)
  3426. {
  3427. return acos(x.w) * T(2) * T(RAD2DEG);
  3428. }
  3429. template <typename T>
  3430. tvec3<T> axis(tquat<T> const & x)
  3431. {
  3432. T tmp1 = T(1) - x.w * x.w;
  3433. if(tmp1 <= T(0))
  3434. {
  3435. return tvec3<T>(0, 0, 1);
  3436. }
  3437. T tmp2 = T(1) / sqrt(tmp1);
  3438. return tvec3<T>(x.x * tmp2, x.y * tmp2, x.z * tmp2);
  3439. }
  3440. template <typename valType>
  3441. tquat<valType> angleAxis(valType angle, tvec3<valType> const & axis)
  3442. {
  3443. tquat<valType> result;
  3444. valType a = (valType)(valType(DEG2RAD(angle)));
  3445. valType s = sin(a * valType(0.5));
  3446. result.w = cos(a * valType(0.5));
  3447. result.x = axis.x * s;
  3448. result.y = axis.y * s;
  3449. result.z = axis.z * s;
  3450. return result;
  3451. }
  3452. ////////////////////////////////////////////////////////////////////////////////////////////////////////
  3453. ////////////////////////////////////////////////////////////////////////////////////////////////////////
  3454. ////////////////////////////////////////////////////////////////////////////////////////////////////////
  3455. template <typename T>
  3456. tmat4x4<T> translate(tmat4x4<T> const & m,tvec3<T> const & v)
  3457. {
  3458. tmat4x4<T> res(m);
  3459. res[3] = m[0] * v[0] + m[1] * v[1] + m[2] * v[2] + m[3];
  3460. return res;
  3461. }
  3462. template <typename T>
  3463. tmat4x4<T> rotate
  3464. (
  3465. tmat4x4<T> const & m,
  3466. T const & angle,
  3467. tvec3<T> const & v
  3468. )
  3469. {
  3470. T a = DEG2RAD(angle);
  3471. T c = cos(a);
  3472. T s = sin(a);
  3473. tvec3<T> axis = normalize(v);
  3474. tvec3<T> temp = (T(1) - c) * axis;
  3475. tmat4x4<T> res;
  3476. res[0][0] = c + temp[0] * axis[0];
  3477. res[0][1] = 0 + temp[0] * axis[1] + s * axis[2];
  3478. res[0][2] = 0 + temp[0] * axis[2] - s * axis[1];
  3479. res[1][0] = 0 + temp[1] * axis[0] - s * axis[2];
  3480. res[1][1] = c + temp[1] * axis[1];
  3481. res[1][2] = 0 + temp[1] * axis[2] + s * axis[0];
  3482. res[2][0] = 0 + temp[2] * axis[0] + s * axis[1];
  3483. res[2][1] = 0 + temp[2] * axis[1] - s * axis[0];
  3484. res[2][2] = c + temp[2] * axis[2];
  3485. tmat4x4<T> rot;
  3486. rot[0] = m[0] * res[0][0] + m[1] * res[0][1] + m[2] * res[0][2];
  3487. rot[1] = m[0] * res[1][0] + m[1] * res[1][1] + m[2] * res[1][2];
  3488. rot[2] = m[0] * res[2][0] + m[1] * res[2][1] + m[2] * res[2][2];
  3489. rot[3] = m[3];
  3490. return rot;
  3491. }
  3492. template <typename T>
  3493. tmat4x4<T> scale(tmat4x4<T> const & m,tvec3<T> const & v)
  3494. {
  3495. tmat4x4<T> res;
  3496. res[0] = m[0] * v[0];
  3497. res[1] = m[1] * v[1];
  3498. res[2] = m[2] * v[2];
  3499. res[3] = m[3];
  3500. return res;
  3501. }
  3502. template <typename T>
  3503. tmat4x4<T> rotate_slow
  3504. (
  3505. tmat4x4<T> const & m,
  3506. T const & angle,
  3507. tvec3<T> const & v
  3508. )
  3509. {
  3510. T const a = DEG2RAD(angle);
  3511. T c = cos(a);
  3512. T s = sin(a);
  3513. tmat4x4<T> res;
  3514. tvec3<T> axis = normalize(v);
  3515. res[0][0] = c + (1 - c) * axis.x * axis.x;
  3516. res[0][1] = (1 - c) * axis.x * axis.y + s * axis.z;
  3517. res[0][2] = (1 - c) * axis.x * axis.z - s * axis.y;
  3518. res[0][3] = 0;
  3519. res[1][0] = (1 - c) * axis.y * axis.x - s * axis.z;
  3520. res[1][1] = c + (1 - c) * axis.y * axis.y;
  3521. res[1][2] = (1 - c) * axis.y * axis.z + s * axis.x;
  3522. res[1][3] = 0;
  3523. res[2][0] = (1 - c) * axis.z * axis.x + s * axis.y;
  3524. res[2][1] = (1 - c) * axis.z * axis.y - s * axis.x;
  3525. res[2][2] = c + (1 - c) * axis.z * axis.z;
  3526. res[2][3] = 0;
  3527. res[3] = tvec4<T>(0, 0, 0, 1);
  3528. return m * res;
  3529. }
  3530. template <typename T>
  3531. tmat4x4<T> scale_slow(tmat4x4<T> const & m,tvec3<T> const & v)
  3532. {
  3533. tmat4x4<T> res(T(1));
  3534. res[0][0] = v.x;
  3535. res[1][1] = v.y;
  3536. res[2][2] = v.z;
  3537. return m * res;
  3538. }
  3539. template <typename valType>
  3540. tmat4x4<valType> ortho
  3541. (
  3542. valType left,
  3543. valType right,
  3544. valType bottom,
  3545. valType top,
  3546. valType zNear,
  3547. valType zFar
  3548. )
  3549. {
  3550. tmat4x4<valType> res(1);
  3551. res[0][0] = valType(2) / (right - left);
  3552. res[1][1] = valType(2) / (top - bottom);
  3553. res[2][2] = - valType(2) / (zFar - zNear);
  3554. res[3][0] = - (right + left) / (right - left);
  3555. res[3][1] = - (top + bottom) / (top - bottom);
  3556. res[3][2] = - (zFar + zNear) / (zFar - zNear);
  3557. return res;
  3558. }
  3559. template <typename valType>
  3560. tmat4x4<valType> frustum
  3561. (
  3562. valType left,
  3563. valType right,
  3564. valType bottom,
  3565. valType top,
  3566. valType nearVal,
  3567. valType farVal
  3568. )
  3569. {
  3570. tmat4x4<valType> res(0);
  3571. res[0][0] = (valType(2) * nearVal) / (right - left);
  3572. res[1][1] = (valType(2) * nearVal) / (top - bottom);
  3573. res[2][0] = (right + left) / (right - left);
  3574. res[2][1] = (top + bottom) / (top - bottom);
  3575. res[2][2] = -(farVal + nearVal) / (farVal - nearVal);
  3576. res[2][3] = valType(-1);
  3577. res[3][2] = -(valType(2) * farVal * nearVal) / (farVal - nearVal);
  3578. return res;
  3579. }
  3580. template <typename valType>
  3581. tmat4x4<valType> perspective(valType fovy, valType aspect, valType zNear, valType zFar)
  3582. {
  3583. valType range = tan(fovy * valType(DEG2RAD(0.5))) * zNear;
  3584. valType left = -range * aspect;
  3585. valType right = range * aspect;
  3586. valType bottom = -range;
  3587. valType top = range;
  3588. tmat4x4<valType> res(valType(0));
  3589. res[0][0] = (valType(2) * zNear) / (right - left);
  3590. res[1][1] = (valType(2) * zNear) / (top - bottom);
  3591. res[2][2] = - (zFar + zNear) / (zFar - zNear);
  3592. res[2][3] = - valType(1);
  3593. res[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear);
  3594. return res;
  3595. }
  3596. template <typename T>
  3597. tvec3<T> project
  3598. (
  3599. tvec3<T> const & obj,
  3600. tmat4x4<T> const & model,
  3601. tmat4x4<T> const & proj,
  3602. tvec4<T> const & viewport
  3603. )
  3604. {
  3605. tvec4<T> tmp = tvec4<T>(obj.x, obj.y,obj.z,T(1));
  3606. tmp = model * tmp;
  3607. tmp = proj * tmp;
  3608. tmp /= tmp.w;
  3609. tmp = tmp * T(0.5) + T(0.5);
  3610. tmp[0] = tmp[0] * T(viewport[2]) + T(viewport[0]);
  3611. tmp[1] = tmp[1] * T(viewport[3]) + T(viewport[1]);
  3612. return tvec3<T>(tmp.x,tmp.y,tmp.z);
  3613. }
  3614. template <typename T>
  3615. tvec3<T> unProject
  3616. (
  3617. tvec3<T> const & win,
  3618. tmat4x4<T> const & model,
  3619. tmat4x4<T> const & proj,
  3620. tvec4<T> const & viewport
  3621. )
  3622. {
  3623. tmat4x4<T> inverses = (proj * model).inverse();
  3624. tvec4<T> tmp = tvec4<T>(win.x,win.y,win.z, T(1));
  3625. tmp.x = (tmp.x - T(viewport[0])) / T(viewport[2]);
  3626. tmp.y = (tmp.y - T(viewport[1])) / T(viewport[3]);
  3627. tmp = tmp * T(2) - T(1);
  3628. tvec4<T> obj = inverses * tmp;
  3629. obj /= obj.w;
  3630. return tvec3<T>(obj.x,obj.y,obj.z);
  3631. }
  3632. template <typename T>
  3633. tmat4x4<T> pickMatrix
  3634. (
  3635. tvec2<T> const & center,
  3636. tvec2<T> const & delta,
  3637. tvec4<T> const & viewport
  3638. )
  3639. {
  3640. assert(delta.x > T(0) && delta.y > T(0));
  3641. tmat4x4<T> res(1.0f);
  3642. if(!(delta.x > T(0) && delta.y > T(0)))
  3643. {
  3644. return res;
  3645. }
  3646. tvec3<T> Temp
  3647. (
  3648. (T(viewport[2]) - T(2) * (center.x - T(viewport[0]))) / delta.x,
  3649. (T(viewport[3]) - T(2) * (center.y - T(viewport[1]))) / delta.y,
  3650. T(0)
  3651. );
  3652. res = translate(res, Temp);
  3653. return scale(res, tvec3<T>(T(viewport[2]) / delta.x, T(viewport[3]) / delta.y, T(1)));
  3654. }
  3655. template <typename T>
  3656. tmat4x4<T> lookAt
  3657. (
  3658. tvec3<T> const & eye,
  3659. tvec3<T> const & center,
  3660. tvec3<T> const & up
  3661. )
  3662. {
  3663. tvec3<T> f = normalize(center - eye);
  3664. tvec3<T> u = normalize(up);
  3665. tvec3<T> s = normalize(cross(f, u));
  3666. u = cross(s, f);
  3667. tmat4x4<T> res(1);
  3668. res[0][0] = s.x;
  3669. res[1][0] = s.y;
  3670. res[2][0] = s.z;
  3671. res[0][1] = u.x;
  3672. res[1][1] = u.y;
  3673. res[2][1] = u.z;
  3674. res[0][2] = -f.x;
  3675. res[1][2] = -f.y;
  3676. res[2][2] = -f.z;
  3677. res[3][0] = -dot(s, eye);
  3678. res[3][1] = -dot(u, eye);
  3679. res[3][2] = dot(f, eye);
  3680. return res;
  3681. }
  3682. template<typename T>
  3683. class AxisAlignedBox2D
  3684. {
  3685. public:
  3686. enum Extent
  3687. {
  3688. EXTENT_NULL,
  3689. EXTENT_FINITE,
  3690. EXTENT_INFINITE
  3691. };
  3692. public:
  3693. tvec2<T> _minimum;
  3694. tvec2<T> _maximum;
  3695. Extent _extent;
  3696. public:
  3697. /*
  3698. 1-----2
  3699. /| /|
  3700. / | / |
  3701. 5-----4 |
  3702. | 0--|--3
  3703. | / | /
  3704. |/ |/
  3705. 6-----7
  3706. */
  3707. typedef enum
  3708. {
  3709. FAR_LEFT_BOTTOM = 0,
  3710. FAR_LEFT_TOP = 1,
  3711. FAR_RIGHT_TOP = 2,
  3712. FAR_RIGHT_BOTTOM = 3,
  3713. NEAR_RIGHT_BOTTOM = 7,
  3714. NEAR_LEFT_BOTTOM = 6,
  3715. NEAR_LEFT_TOP = 5,
  3716. NEAR_RIGHT_TOP = 4
  3717. } CornerEnum;
  3718. AxisAlignedBox2D()
  3719. {
  3720. _minimum = tvec2<T>( T(-0.5), T(-0.5));
  3721. _maximum = tvec2<T>( T(0.5), T(0.5));
  3722. _extent = EXTENT_NULL;
  3723. }
  3724. AxisAlignedBox2D(const AxisAlignedBox2D & rkBox)
  3725. {
  3726. setExtents( rkBox._minimum, rkBox._maximum );
  3727. _extent = rkBox._extent;
  3728. }
  3729. AxisAlignedBox2D( const tvec2<T>& min, const tvec2<T>& max )
  3730. {
  3731. setExtents( min, max );
  3732. }
  3733. AxisAlignedBox2D(
  3734. T mx, T my,
  3735. T Mx, T My
  3736. )
  3737. {
  3738. setExtents( tvec2<T>(mx, my), tvec2<T>(Mx, My));
  3739. }
  3740. AxisAlignedBox2D<T>& operator=(const AxisAlignedBox2D<T>& right)
  3741. {
  3742. setExtents(right._minimum, right._maximum);
  3743. return *this;
  3744. }
  3745. ~AxisAlignedBox2D()
  3746. {
  3747. }
  3748. /**
  3749. * Gets the minimum corner of the box.
  3750. */
  3751. const tvec2<T>& getMinimum(void) const
  3752. {
  3753. return _minimum;
  3754. }
  3755. /**
  3756. * Gets a modifiable version of the minimum
  3757. * corner of the box.
  3758. */
  3759. tvec2<T>& getMinimum(void)
  3760. {
  3761. return _minimum;
  3762. }
  3763. void setMinimum( const tvec2<T>& vec )
  3764. {
  3765. _minimum = vec;
  3766. }
  3767. void setMinimum( T x,T y )
  3768. {
  3769. _minimum = tvec2<T>(x,y);
  3770. }
  3771. /**
  3772. * Gets the maximum corner of the box.
  3773. */
  3774. const tvec2<T>& getMaximum(void) const
  3775. {
  3776. return _maximum;
  3777. }
  3778. /**
  3779. * Gets a modifiable version of the maximum
  3780. * corner of the box.
  3781. */
  3782. tvec2<T>& getMaximum(void)
  3783. {
  3784. return _maximum;
  3785. }
  3786. /**
  3787. * Sets the maximum corner of the box.
  3788. */
  3789. void setMaximum( const tvec2<T>& vec )
  3790. {
  3791. _maximum = vec;
  3792. }
  3793. void setMaximum( T x, T y )
  3794. {
  3795. _maximum.x = x;
  3796. _maximum.y = y;
  3797. }
  3798. /**
  3799. * Sets both minimum and maximum extents at once.
  3800. */
  3801. void setExtents( const tvec2<T>& min, const tvec2<T>& max )
  3802. {
  3803. _minimum = min;
  3804. _maximum = max;
  3805. _extent = EXTENT_FINITE;
  3806. }
  3807. void setExtents(
  3808. T mx, T my,
  3809. T Mx, T My
  3810. )
  3811. {
  3812. _minimum.x = mx;
  3813. _minimum.y = my;
  3814. _maximum.x = Mx;
  3815. _maximum.y = My;
  3816. _extent = EXTENT_FINITE;
  3817. }
  3818. inline bool intersects(const AxisAlignedBox2D& b2) const
  3819. {
  3820. if (_maximum.x < b2._minimum.x)
  3821. return false;
  3822. if (_maximum.y < b2._minimum.y)
  3823. return false;
  3824. if (_minimum.x > b2._maximum.x)
  3825. return false;
  3826. if (_minimum.y > b2._maximum.y)
  3827. return false;
  3828. return true;
  3829. }
  3830. inline AxisAlignedBox2D<T> intersection(const AxisAlignedBox2D<T>& b2) const
  3831. {
  3832. tvec2<T> intMin = _minimum;
  3833. tvec2<T> intMax = _maximum;
  3834. intMin.makeCeil(b2.getMinimum());
  3835. intMax.makeFloor(b2.getMaximum());
  3836. if (intMin.x < intMax.x &&
  3837. intMin.y < intMax.y)
  3838. {
  3839. return AxisAlignedBox2D<T>(intMin, intMax);
  3840. }
  3841. return AxisAlignedBox2D<T>();
  3842. }
  3843. inline void setNull()
  3844. {
  3845. _extent = EXTENT_NULL;
  3846. }
  3847. inline bool isNull(void) const
  3848. {
  3849. return (_extent == EXTENT_NULL);
  3850. }
  3851. bool isFinite(void) const
  3852. {
  3853. return (_extent == EXTENT_FINITE);
  3854. }
  3855. inline void setInfinite()
  3856. {
  3857. _extent = EXTENT_INFINITE;
  3858. }
  3859. inline bool isInfinite(void) const
  3860. {
  3861. return (_extent == EXTENT_INFINITE);
  3862. }
  3863. inline bool intersects(const tvec2<T>& v) const
  3864. {
  3865. return( v.x >= _minimum.x && v.x <= _maximum.x &&
  3866. v.y >= _minimum.y && v.y <= _maximum.y );
  3867. }
  3868. inline tvec2<T> getCenter(void) const
  3869. {
  3870. return tvec2<T>(
  3871. (_maximum.x + _minimum.x) * T(0.5f),
  3872. (_maximum.y + _minimum.y) * T(0.5f));
  3873. }
  3874. /**
  3875. * Gets the size of the box
  3876. */
  3877. inline tvec2<T> getSize(void) const
  3878. {
  3879. return _maximum - _minimum;
  3880. }
  3881. inline tvec2<T> getHalfSize(void) const
  3882. {
  3883. return (_maximum - _minimum) * T(0.5);
  3884. }
  3885. inline bool contains(const tvec2<T>& v) const
  3886. {
  3887. return _minimum.x <= v.x && v.x <= _maximum.x &&
  3888. _minimum.y <= v.y && v.y <= _maximum.y;
  3889. }
  3890. inline bool contains(const AxisAlignedBox2D& other) const
  3891. {
  3892. return this->_minimum.x <= other._minimum.x &&
  3893. this->_minimum.y <= other._minimum.y &&
  3894. other._maximum.x <= this->_maximum.x &&
  3895. other._maximum.y <= this->_maximum.y;
  3896. }
  3897. inline bool operator== (const AxisAlignedBox2D& right) const
  3898. {
  3899. return this->_minimum == right._minimum &&
  3900. this->_maximum == right._maximum;
  3901. }
  3902. inline bool operator!= (const AxisAlignedBox2D& right) const
  3903. {
  3904. return !(*this == right);
  3905. }
  3906. inline void merge(tvec2<T> point)
  3907. {
  3908. if (_minimum.x > point.x)
  3909. {
  3910. _minimum.x = point.x;
  3911. }
  3912. if (_minimum.y > point.y)
  3913. {
  3914. _minimum.y = point.y;
  3915. }
  3916. if (_maximum.x < point.x)
  3917. {
  3918. _maximum.x = point.x;
  3919. }
  3920. if (_maximum.y < point.y)
  3921. {
  3922. _maximum.y = point.y;
  3923. }
  3924. }
  3925. inline void merge(AxisAlignedBox2D<T> other)
  3926. {
  3927. _maximum.makeCeil(other._maximum);
  3928. _minimum.makeFloor(other._minimum);
  3929. }
  3930. };
  3931. template<typename T>
  3932. class AxisAlignedBox
  3933. {
  3934. public:
  3935. enum Extent
  3936. {
  3937. EXTENT_NULL,
  3938. EXTENT_FINITE,
  3939. EXTENT_INFINITE
  3940. };
  3941. public:
  3942. tvec3<T> _minimum;
  3943. tvec3<T> _maximum;
  3944. Extent _extent;
  3945. public:
  3946. /*
  3947. 1-----2
  3948. /| /|
  3949. / | / |
  3950. 5-----4 |
  3951. | 0--|--3
  3952. | / | /
  3953. |/ |/
  3954. 6-----7
  3955. */
  3956. typedef enum
  3957. {
  3958. FAR_LEFT_BOTTOM = 0,
  3959. FAR_LEFT_TOP = 1,
  3960. FAR_RIGHT_TOP = 2,
  3961. FAR_RIGHT_BOTTOM = 3,
  3962. NEAR_RIGHT_BOTTOM = 7,
  3963. NEAR_LEFT_BOTTOM = 6,
  3964. NEAR_LEFT_TOP = 5,
  3965. NEAR_RIGHT_TOP = 4
  3966. } CornerEnum;
  3967. AxisAlignedBox()
  3968. {
  3969. _minimum = tvec3<T>( T(-0.5), T(-0.5), T(-0.5) );
  3970. _maximum = tvec3<T>( T(0.5), T(0.5), T(0.5) );
  3971. _extent = EXTENT_NULL;
  3972. }
  3973. AxisAlignedBox(const AxisAlignedBox & rkBox)
  3974. {
  3975. setExtents( rkBox._minimum, rkBox._maximum );
  3976. _extent = rkBox._extent;
  3977. }
  3978. AxisAlignedBox( const tvec3<T>& min, const tvec3<T>& max )
  3979. {
  3980. setExtents( min, max );
  3981. }
  3982. AxisAlignedBox(
  3983. T mx, T my, T mz,
  3984. T Mx, T My, T Mz
  3985. )
  3986. {
  3987. setExtents( mx, my, mz, Mx, My, Mz );
  3988. }
  3989. AxisAlignedBox<T>& operator=(const AxisAlignedBox<T>& right)
  3990. {
  3991. setExtents(right._minimum, right._maximum);
  3992. return *this;
  3993. }
  3994. ~AxisAlignedBox()
  3995. {
  3996. }
  3997. /**
  3998. * Gets the minimum corner of the box.
  3999. */
  4000. const tvec3<T>& getMinimum(void) const
  4001. {
  4002. return _minimum;
  4003. }
  4004. /**
  4005. * Gets a modifiable version of the minimum
  4006. * corner of the box.
  4007. */
  4008. tvec3<T>& getMinimum(void)
  4009. {
  4010. return _minimum;
  4011. }
  4012. void setMinimum(const tvec3<T>& mins)
  4013. {
  4014. _minimum = mins;
  4015. }
  4016. void setMinimum(T x,T y, T z)
  4017. {
  4018. _minimum = tvec3<T>(x,y,z);
  4019. }
  4020. /**
  4021. * Gets the maximum corner of the box.
  4022. */
  4023. const tvec3<T>& getMaximum(void) const
  4024. {
  4025. return _maximum;
  4026. }
  4027. /**
  4028. * Gets a modifiable version of the maximum
  4029. * corner of the box.
  4030. */
  4031. tvec3<T>& getMaximum(void)
  4032. {
  4033. return _maximum;
  4034. }
  4035. /**
  4036. * Sets the maximum corner of the box.
  4037. */
  4038. void setMaximum( const tvec3<T>& vec )
  4039. {
  4040. _maximum = vec;
  4041. }
  4042. void setMaximum( T x, T y, T z )
  4043. {
  4044. _maximum.x = x;
  4045. _maximum.y = y;
  4046. _maximum.z = z;
  4047. }
  4048. /**
  4049. * Changes one of the components of the maximum corner of the box
  4050. * used to resize only one dimension of the box
  4051. */
  4052. void setMaximumX( T x )
  4053. {
  4054. _maximum.x = x;
  4055. }
  4056. void setMaximumY( T y )
  4057. {
  4058. _maximum.y = y;
  4059. }
  4060. void setMaximumZ( T z )
  4061. {
  4062. _maximum.z = z;
  4063. }
  4064. /**
  4065. * Sets both minimum and maximum extents at once.
  4066. */
  4067. void setExtents( const tvec3<T>& min, const tvec3<T>& max )
  4068. {
  4069. _minimum = min;
  4070. _maximum = max;
  4071. _extent = EXTENT_FINITE;
  4072. }
  4073. void setExtents(
  4074. T mx, T my, T mz,
  4075. T Mx, T My, T Mz )
  4076. {
  4077. _minimum.x = mx;
  4078. _minimum.y = my;
  4079. _minimum.z = mz;
  4080. _maximum.x = Mx;
  4081. _maximum.y = My;
  4082. _maximum.z = Mz;
  4083. _extent = EXTENT_FINITE;
  4084. }
  4085. /** Returns a pointer to an array of 8 corner points, useful for
  4086. collision vs. non-aligned objects.
  4087. @remarks
  4088. If the order of these corners is important, they are as
  4089. follows: The 4 points of the minimum Z face (note that
  4090. because Ogre uses right-handed coordinates, the minimum Z is
  4091. at the 'back' of the box) starting with the minimum point of
  4092. all, then anticlockwise around this face (if you are looking
  4093. onto the face from outside the box). Then the 4 points of the
  4094. maximum Z face, starting with maximum point of all, then
  4095. anticlockwise around this face (looking onto the face from
  4096. outside the box). Like this:
  4097. <pre>
  4098. 1-----2
  4099. /| /|
  4100. / | / |
  4101. 5-----4 |
  4102. | 0--|--3
  4103. | / | /
  4104. |/ |/
  4105. 6-----7
  4106. </pre>
  4107. @remarks as this implementation uses a static member, make sure to use your own copy !
  4108. */
  4109. void getAllCorners(tvec3<T> mpCorners[8] ) const
  4110. {
  4111. mpCorners[0] = _minimum;
  4112. mpCorners[1].x = _minimum.x; mpCorners[1].y = _maximum.y; mpCorners[1].z = _minimum.z;
  4113. mpCorners[2].x = _maximum.x; mpCorners[2].y = _maximum.y; mpCorners[2].z = _minimum.z;
  4114. mpCorners[3].x = _maximum.x; mpCorners[3].y = _minimum.y; mpCorners[3].z = _minimum.z;
  4115. mpCorners[4] = _maximum;
  4116. mpCorners[5].x = _minimum.x; mpCorners[5].y = _maximum.y; mpCorners[5].z = _maximum.z;
  4117. mpCorners[6].x = _minimum.x; mpCorners[6].y = _minimum.y; mpCorners[6].z = _maximum.z;
  4118. mpCorners[7].x = _maximum.x; mpCorners[7].y = _minimum.y; mpCorners[7].z = _maximum.z;
  4119. }
  4120. /**
  4121. * gets the position of one of the corners
  4122. */
  4123. tvec3<T> getCorner(CornerEnum cornerToGet) const
  4124. {
  4125. switch(cornerToGet)
  4126. {
  4127. case FAR_LEFT_BOTTOM:
  4128. return _minimum;
  4129. case FAR_LEFT_TOP:
  4130. return tvec3<T>(_minimum.x, _maximum.y, _minimum.z);
  4131. case FAR_RIGHT_TOP:
  4132. return tvec3<T>(_maximum.x, _maximum.y, _minimum.z);
  4133. case FAR_RIGHT_BOTTOM:
  4134. return tvec3<T>(_maximum.x, _minimum.y, _minimum.z);
  4135. case NEAR_RIGHT_BOTTOM:
  4136. return tvec3<T>(_maximum.x, _minimum.y, _maximum.z);
  4137. case NEAR_LEFT_BOTTOM:
  4138. return tvec3<T>(_minimum.x, _minimum.y, _maximum.z);
  4139. case NEAR_LEFT_TOP:
  4140. return tvec3<T>(_minimum.x, _maximum.y, _maximum.z);
  4141. case NEAR_RIGHT_TOP:
  4142. return _maximum;
  4143. default:
  4144. return tvec3<T>();
  4145. }
  4146. }
  4147. /**
  4148. * Merges the passed in box into the current box. The result is the
  4149. * box which encompasses both.
  4150. */
  4151. void merge( const AxisAlignedBox<T>& right )
  4152. {
  4153. if ((right._extent == EXTENT_NULL) || (_extent == EXTENT_INFINITE))
  4154. {
  4155. return;
  4156. }
  4157. else if (right._extent == EXTENT_INFINITE)
  4158. {
  4159. _extent = EXTENT_INFINITE;
  4160. }
  4161. else if (_extent == EXTENT_NULL)
  4162. {
  4163. setExtents(right._minimum, right._maximum);
  4164. }
  4165. else
  4166. {
  4167. //! merge
  4168. tvec3<T> min = _minimum;
  4169. tvec3<T> max = _maximum;
  4170. max.makeCeil(right._maximum);
  4171. min.makeFloor(right._minimum);
  4172. setExtents(min, max);
  4173. }
  4174. }
  4175. /**
  4176. * Extends the box to encompass the specified point (if needed).
  4177. */
  4178. void merge( const tvec3<T>& point )
  4179. {
  4180. switch (_extent)
  4181. {
  4182. case EXTENT_NULL: // if null, use this point
  4183. setExtents(point, point);
  4184. return;
  4185. case EXTENT_FINITE:
  4186. _maximum.makeCeil(point);
  4187. _minimum.makeFloor(point);
  4188. return;
  4189. case EXTENT_INFINITE:
  4190. return;
  4191. }
  4192. }
  4193. void transform( const tmat4x4<T>& matrix )
  4194. {
  4195. tvec3<T> oldMin;
  4196. tvec3<T> oldMax;
  4197. tvec3<T> currentCorner;
  4198. oldMin = _minimum;
  4199. oldMax = _maximum;
  4200. // We sequentially compute the corners in the following order :
  4201. // 0, 6, 5, 1, 2, 4 ,7 , 3
  4202. // This sequence allows us to only change one member at a time to get at all corners.
  4203. // For each one, we transform it using the matrix
  4204. // Which gives the resulting point and merge the resulting point.
  4205. currentCorner = oldMin;
  4206. tvec3<T> vVert = currentCorner * matrix;
  4207. setExtents(vVert,vVert);
  4208. // First corner
  4209. // min min min
  4210. currentCorner = oldMin;
  4211. merge( currentCorner * matrix );
  4212. // min,min,max
  4213. currentCorner.z = oldMax.z;
  4214. merge( currentCorner * matrix );
  4215. // min max max
  4216. currentCorner.y = oldMax.y;
  4217. merge( currentCorner * matrix );
  4218. // min max min
  4219. currentCorner.z = oldMin.z;
  4220. merge( currentCorner * matrix );
  4221. // max max min
  4222. currentCorner.x = oldMax.x;
  4223. merge( currentCorner * matrix );
  4224. // max max max
  4225. currentCorner.z = oldMax.z;
  4226. merge( currentCorner * matrix );
  4227. // max min max
  4228. currentCorner.y = oldMin.y;
  4229. merge( currentCorner * matrix);
  4230. // max min min
  4231. currentCorner.z = oldMin.z;
  4232. merge( currentCorner * matrix);
  4233. }
  4234. /**
  4235. * Returns whether or not this box intersects another.
  4236. */
  4237. bool intersects(const AxisAlignedBox& b2) const
  4238. {
  4239. if (_maximum.x < b2._minimum.x)
  4240. return false;
  4241. if (_maximum.y < b2._minimum.y)
  4242. return false;
  4243. if (_maximum.z < b2._minimum.z)
  4244. return false;
  4245. if (_minimum.x > b2._maximum.x)
  4246. return false;
  4247. if (_minimum.y > b2._maximum.y)
  4248. return false;
  4249. if (_minimum.z > b2._maximum.z)
  4250. return false;
  4251. return true;
  4252. }
  4253. /**
  4254. * Returns whether or not this box intersects another.
  4255. */
  4256. bool intersectsNoZ(const AxisAlignedBox& b2) const
  4257. {
  4258. if (_maximum.x < b2._minimum.x)
  4259. return false;
  4260. if (_maximum.y < b2._minimum.y)
  4261. return false;
  4262. if (_minimum.x > b2._maximum.x)
  4263. return false;
  4264. if (_minimum.y > b2._maximum.y)
  4265. return false;
  4266. return true;
  4267. }
  4268. AxisAlignedBox<T> intersection(const AxisAlignedBox<T>& b2) const
  4269. {
  4270. tvec3<T> intMin = _minimum;
  4271. tvec3<T> intMax = _maximum;
  4272. intMin.makeCeil(b2.getMinimum());
  4273. intMax.makeFloor(b2.getMaximum());
  4274. if (intMin.x < intMax.x &&
  4275. intMin.y < intMax.y &&
  4276. intMin.z < intMax.z)
  4277. {
  4278. return AxisAlignedBox<T>(intMin, intMax);
  4279. }
  4280. return AxisAlignedBox<T>();
  4281. }
  4282. void setNull()
  4283. {
  4284. _extent = EXTENT_NULL;
  4285. }
  4286. bool isNull(void) const
  4287. {
  4288. return (_extent == EXTENT_NULL);
  4289. }
  4290. bool isFinite(void) const
  4291. {
  4292. return (_extent == EXTENT_FINITE);
  4293. }
  4294. void setInfinite()
  4295. {
  4296. _extent = EXTENT_INFINITE;
  4297. }
  4298. bool isInfinite(void) const
  4299. {
  4300. return (_extent == EXTENT_INFINITE);
  4301. }
  4302. void scale(const tvec3<T>& s)
  4303. {
  4304. tvec3<T> min = _minimum * s;
  4305. tvec3<T> max = _maximum * s;
  4306. setExtents(min, max);
  4307. }
  4308. bool intersects(const tvec3<T>& v) const
  4309. {
  4310. return( v.x >= _minimum.x && v.x <= _maximum.x &&
  4311. v.y >= _minimum.y && v.y <= _maximum.y &&
  4312. v.z >= _minimum.z && v.z <= _maximum.z);
  4313. }
  4314. bool intersects(const tvec2<T>& v) const
  4315. {
  4316. return( v.x >= _minimum.x && v.x <= _maximum.x &&
  4317. v.y >= _minimum.y && v.y <= _maximum.y );
  4318. }
  4319. tvec3<T> getCenter(void) const
  4320. {
  4321. return tvec3<T>(
  4322. (_maximum.x + _minimum.x) * T(0.5f),
  4323. (_maximum.y + _minimum.y) * T(0.5f),
  4324. (_maximum.z + _minimum.z) * T(0.5f)
  4325. );
  4326. }
  4327. /**
  4328. * Gets the size of the box
  4329. */
  4330. tvec3<T> getSize(void) const
  4331. {
  4332. return _maximum - _minimum;
  4333. }
  4334. tvec3<T> getHalfSize(void) const
  4335. {
  4336. return (_maximum - _minimum) * T(0.5);
  4337. }
  4338. bool contains(const tvec3<T>& v) const
  4339. {
  4340. return _minimum.x <= v.x && v.x <= _maximum.x &&
  4341. _minimum.y <= v.y && v.y <= _maximum.y &&
  4342. _minimum.z <= v.z && v.z <= _maximum.z;
  4343. }
  4344. bool contains(const AxisAlignedBox& other) const
  4345. {
  4346. return this->_minimum.x <= other._minimum.x &&
  4347. this->_minimum.y <= other._minimum.y &&
  4348. this->_minimum.z <= other._minimum.z &&
  4349. other._maximum.x <= this->_maximum.x &&
  4350. other._maximum.y <= this->_maximum.y &&
  4351. other._maximum.z <= this->_maximum.z;
  4352. }
  4353. bool operator== (const AxisAlignedBox& right) const
  4354. {
  4355. return this->_minimum == right._minimum &&
  4356. this->_maximum == right._maximum;
  4357. }
  4358. bool operator!= (const AxisAlignedBox& right) const
  4359. {
  4360. return !(*this == right);
  4361. }
  4362. };
  4363. template<typename T>
  4364. class tspline
  4365. {
  4366. public:
  4367. tspline()
  4368. {
  4369. mCoeffs[0][0] = 2;
  4370. mCoeffs[0][1] = -2;
  4371. mCoeffs[0][2] = 1;
  4372. mCoeffs[0][3] = 1;
  4373. mCoeffs[1][0] = -3;
  4374. mCoeffs[1][1] = 3;
  4375. mCoeffs[1][2] = -2;
  4376. mCoeffs[1][3] = -1;
  4377. mCoeffs[2][0] = 0;
  4378. mCoeffs[2][1] = 0;
  4379. mCoeffs[2][2] = 1;
  4380. mCoeffs[2][3] = 0;
  4381. mCoeffs[3][0] = 1;
  4382. mCoeffs[3][1] = 0;
  4383. mCoeffs[3][2] = 0;
  4384. mCoeffs[3][3] = 0;
  4385. mCoeffs = mCoeffs.transpose();
  4386. mAutoCalc = true;
  4387. }
  4388. ~tspline(){};
  4389. void addPoint(const tvec3<T>& p)
  4390. {
  4391. mPoints.push_back(p);
  4392. if (mAutoCalc)
  4393. {
  4394. recalcTangents();
  4395. }
  4396. }
  4397. const tvec3<T>& getPoint(size_t index) const
  4398. {
  4399. assert (index < mPoints.size() && "Point index is out of bounds!!");
  4400. return mPoints[index];
  4401. }
  4402. tvec3<T>& getPoint(size_t index)
  4403. {
  4404. assert (index < mPoints.size() && "Point index is out of bounds!!");
  4405. return mPoints[index];
  4406. }
  4407. /**
  4408. * ȡ
  4409. */
  4410. size_t getNumPoints(void) const
  4411. {
  4412. return mPoints.size();
  4413. }
  4414. /**
  4415. * еĵ
  4416. */
  4417. void clear(void)
  4418. {
  4419. mPoints.clear();
  4420. mTangents.clear();
  4421. }
  4422. /**
  4423. * µ
  4424. */
  4425. void updatePoint(size_t index, const tvec3<T>& value)
  4426. {
  4427. assert (index < mPoints.size() && "Point index is out of bounds!!");
  4428. mPoints[index] = value;
  4429. if (mAutoCalc)
  4430. {
  4431. recalcTangents();
  4432. }
  4433. }
  4434. /**
  4435. * ֵȡ
  4436. */
  4437. tvec3<T> interpolate(T time) const
  4438. {
  4439. T fSeg = time * (mPoints.size() - 1);
  4440. unsigned segIdx = (unsigned)fSeg;
  4441. // Apportion t
  4442. time = fSeg - segIdx;
  4443. return interpolate(segIdx, time);
  4444. }
  4445. /**
  4446. * ֵ
  4447. */
  4448. tvec3<T> interpolate(size_t fromIndex, T t) const
  4449. {
  4450. // Bounds check
  4451. assert (fromIndex < mPoints.size() && "fromIndex out of bounds");
  4452. if ((fromIndex + 1) == mPoints.size())
  4453. {
  4454. // Duff request, cannot blend to nothing
  4455. // Just return source
  4456. return mPoints[fromIndex];
  4457. }
  4458. // Fast special cases
  4459. if (t == 0.0f)
  4460. {
  4461. return mPoints[fromIndex];
  4462. }
  4463. else if(t == 1.0f)
  4464. {
  4465. return mPoints[fromIndex + 1];
  4466. }
  4467. // float interpolation
  4468. // Form a vector of powers of t
  4469. T t2, t3;
  4470. t2 = t * t;
  4471. t3 = t2 * t;
  4472. tvec4<T> powers(t3, t2, t, 1);
  4473. const tvec3<T>& point1 = mPoints[fromIndex];
  4474. const tvec3<T>& point2 = mPoints[fromIndex+1];
  4475. const tvec3<T>& tan1 = mTangents[fromIndex];
  4476. const tvec3<T>& tan2 = mTangents[fromIndex+1];
  4477. tmat4x4<T> pt;
  4478. pt[0][0] = point1.x;
  4479. pt[0][1] = point1.y;
  4480. pt[0][2] = point1.z;
  4481. pt[0][3] = 1.0f;
  4482. pt[1][0] = point2.x;
  4483. pt[1][1] = point2.y;
  4484. pt[1][2] = point2.z;
  4485. pt[1][3] = 1.0f;
  4486. pt[2][0] = tan1.x;
  4487. pt[2][1] = tan1.y;
  4488. pt[2][2] = tan1.z;
  4489. pt[2][3] = 1.0f;
  4490. pt[3][0] = tan2.x;
  4491. pt[3][1] = tan2.y;
  4492. pt[3][2] = tan2.z;
  4493. pt[3][3] = 1.0f;
  4494. pt = pt.transpose();
  4495. tvec4<T> ret = powers * mCoeffs * pt;
  4496. return tvec3<T>(ret.x, ret.y, ret.z);
  4497. }
  4498. /**
  4499. * Զ
  4500. */
  4501. void setAutoCalculate(bool autoCalc)
  4502. {
  4503. mAutoCalc = autoCalc;
  4504. }
  4505. /**
  4506. *
  4507. */
  4508. void recalcTangents(void)
  4509. {
  4510. size_t i, numPoints;
  4511. bool isClosed;
  4512. numPoints = mPoints.size();
  4513. if (numPoints < 2)
  4514. {
  4515. return;
  4516. }
  4517. if (mPoints[0] == mPoints[numPoints-1])
  4518. {
  4519. isClosed = true;
  4520. }
  4521. else
  4522. {
  4523. isClosed = false;
  4524. }
  4525. mTangents.resize(numPoints);
  4526. for(i = 0; i < numPoints; ++i)
  4527. {
  4528. if (i ==0)
  4529. {
  4530. // Special case start
  4531. if (isClosed)
  4532. {
  4533. // Use numPoints-2 since numPoints-1 is the last point and == [0]
  4534. mTangents[i] = 0.5f * (mPoints[1] - mPoints[numPoints-2]);
  4535. }
  4536. else
  4537. {
  4538. mTangents[i] = 0.5f * (mPoints[1] - mPoints[0]);
  4539. }
  4540. }
  4541. else if (i == numPoints-1)
  4542. {
  4543. if (isClosed)
  4544. {
  4545. mTangents[i] = mTangents[0];
  4546. }
  4547. else
  4548. {
  4549. mTangents[i] = 0.5f * (mPoints[i] - mPoints[i-1]);
  4550. }
  4551. }
  4552. else
  4553. {
  4554. mTangents[i] = 0.5f * (mPoints[i+1] - mPoints[i-1]);
  4555. }
  4556. }
  4557. }
  4558. public:
  4559. bool mAutoCalc;
  4560. std::vector< tvec3<T> > mPoints;
  4561. std::vector< tvec3<T> > mTangents;
  4562. tmat4x4<T> mCoeffs;
  4563. };
  4564. template < typename T >
  4565. class tellipsoidModel
  4566. {
  4567. public:
  4568. tellipsoidModel(T radiusEquator = T(WGS_84_RADIUS_EQUATOR),T radiusPolar = T(WGS_84_RADIUS_POLAR))
  4569. {
  4570. _radiusEquator = radiusEquator;
  4571. _radiusPolar = radiusPolar;
  4572. T flattening = (_radiusEquator-_radiusPolar)/_radiusEquator;
  4573. _eccentricitySquared= T(2)*flattening - flattening*flattening;
  4574. }
  4575. ~tellipsoidModel(void)
  4576. {
  4577. }
  4578. void convertLatLongHeightToXYZ(
  4579. T latitude,
  4580. T longitude,
  4581. T height,
  4582. T& X,
  4583. T& Y,
  4584. T& Z
  4585. ) const
  4586. {
  4587. // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif
  4588. T sin_latitude = sin(latitude);
  4589. T cos_latitude = cos(latitude);
  4590. T N = _radiusEquator / sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
  4591. X = (N+height) * cos_latitude*cos(longitude);
  4592. Y = (N+height) * cos_latitude*sin(longitude);
  4593. Z = (N*(1-_eccentricitySquared)+height)*sin_latitude;
  4594. }
  4595. void convertXYZToLatLongHeight(
  4596. T X,
  4597. T Y,
  4598. T Z,
  4599. T& latitude,
  4600. T& longitude,
  4601. T& height
  4602. ) const
  4603. {
  4604. // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif
  4605. T p = (T)sqrt(X*X + Y*Y);
  4606. T theta = (T)atan2(Z*_radiusEquator , (p*_radiusPolar));
  4607. T eDashSquared = (_radiusEquator*_radiusEquator - _radiusPolar*_radiusPolar) / (_radiusPolar*_radiusPolar);
  4608. T sin_theta = (T)sin(theta);
  4609. T cos_theta = (T)cos(theta);
  4610. latitude = (T)atan( (Z + eDashSquared*_radiusPolar*sin_theta*sin_theta*sin_theta) /
  4611. (p - _eccentricitySquared*_radiusEquator*cos_theta*cos_theta*cos_theta) );
  4612. longitude = (T)atan2(Y,X);
  4613. T sin_latitude = (T)sin(latitude);
  4614. T N = _radiusEquator / (T)sqrt( 1.0 - _eccentricitySquared*sin_latitude*sin_latitude);
  4615. height = p/(T)cos(latitude) - N;
  4616. }
  4617. protected:
  4618. T _radiusEquator;
  4619. T _radiusPolar;
  4620. T _eccentricitySquared;
  4621. };
  4622. class Rgba4Byte
  4623. {
  4624. public:
  4625. Rgba4Byte(
  4626. unsigned char r = 255,
  4627. unsigned char g = 255,
  4628. unsigned char b = 255,
  4629. unsigned char a = 255
  4630. )
  4631. {
  4632. _r = r;
  4633. _g = g;
  4634. _b = b;
  4635. _a = a;
  4636. }
  4637. Rgba4Byte(uint rgba)
  4638. {
  4639. _color = rgba;
  4640. }
  4641. friend bool operator == (const Rgba4Byte& left,const Rgba4Byte& right)
  4642. {
  4643. return left._r==right._r &&
  4644. left._g==right._g &&
  4645. left._b==right._b &&
  4646. left._a==right._a;
  4647. }
  4648. friend bool operator != (const Rgba4Byte& left,const Rgba4Byte& right)
  4649. {
  4650. return left._r !=right._r ||
  4651. left._g!=right._g ||
  4652. left._b!=right._b ||
  4653. left._a!=right._a;
  4654. }
  4655. friend Rgba4Byte operator + (const Rgba4Byte& left,const Rgba4Byte& right)
  4656. {
  4657. return Rgba4Byte(left._r * right._r
  4658. ,left._g * right._g
  4659. ,left._b * right._b
  4660. ,left._a * right._a);
  4661. }
  4662. operator unsigned()
  4663. {
  4664. return _color;
  4665. }
  4666. uint toUint()
  4667. {
  4668. return _color;
  4669. }
  4670. public:
  4671. union
  4672. {
  4673. struct
  4674. {
  4675. unsigned char _b;
  4676. unsigned char _g;
  4677. unsigned char _r;
  4678. unsigned char _a;
  4679. };
  4680. uint _color;
  4681. };
  4682. };
  4683. typedef Rgba4Byte Rgba;
  4684. inline Rgba4Byte colorLerp(const Rgba4Byte& c1, const Rgba4Byte& c2, float s)
  4685. {
  4686. Rgba4Byte color;
  4687. color._r = (unsigned char)(c1._r + s * (c2._r - c1._r));
  4688. color._g = (unsigned char)(c1._g + s * (c2._g - c1._g));
  4689. color._b = (unsigned char)(c1._b + s * (c2._b - c1._b));
  4690. color._a = (unsigned char)(c1._a + s * (c2._a - c1._a));
  4691. return color;
  4692. }
  4693. inline tvec2<float> uvLerp(const tvec2<float>& c1, const tvec2<float>& c2, float s)
  4694. {
  4695. tvec2<float> color;
  4696. color.x = (c1.x + s * (c2.x - c1.x));
  4697. color.y = (c1.y + s * (c2.y - c1.y));
  4698. return color;
  4699. }
  4700. template <typename T>
  4701. class tAxisAlignedBox2
  4702. {
  4703. public:
  4704. enum Extent
  4705. {
  4706. EXTENT_NULL,
  4707. EXTENT_FINITE,
  4708. EXTENT_INFINITE
  4709. };
  4710. public:
  4711. tvec2<T> _vMin;
  4712. tvec2<T> _vMax;
  4713. Extent mExtent;
  4714. public:
  4715. tvec2<T> center() const
  4716. {
  4717. return (_vMin + _vMax) * T(0.5);
  4718. }
  4719. tvec2<T> size() const
  4720. {
  4721. return _vMax - _vMin;
  4722. }
  4723. tvec2<T> halfSize() const
  4724. {
  4725. return (_vMax - _vMin) * T(0.5);
  4726. }
  4727. bool intersects(tvec2<T> v) const
  4728. {
  4729. return( v.x >= _vMin.x && v.x <= _vMax.x &&
  4730. v.y >= _vMin.y && v.y <= _vMax.y );
  4731. }
  4732. void merge(tvec2<T> point)
  4733. {
  4734. if (_vMin.x > point.x)
  4735. {
  4736. _vMin.x = point.x;
  4737. }
  4738. if (_vMin.y > point.y)
  4739. {
  4740. _vMin.y = point.y;
  4741. }
  4742. if (_vMax.x < point.x)
  4743. {
  4744. _vMax.x = point.x;
  4745. }
  4746. if (_vMax.y < point.y)
  4747. {
  4748. _vMax.y = point.y;
  4749. }
  4750. }
  4751. void merge(tAxisAlignedBox2<T> other)
  4752. {
  4753. _vMax.makeCeil(other._vMax);
  4754. _vMin.makeFloor(other._vMin);
  4755. }
  4756. bool contains(tvec2<T> point) const
  4757. {
  4758. return _vMin.x <= point.x && point.x <= _vMax.x &&
  4759. _vMin.y <= point.y && point.y <= _vMax.y ;
  4760. }
  4761. bool contains(tAxisAlignedBox2<T> other) const
  4762. {
  4763. return this->_vMin.x <= other._vMin.x &&
  4764. this->_vMin.y <= other._vMin.y &&
  4765. other._vMax.x <= this->_vMax.x &&
  4766. other._vMax.y <= this->_vMax.y ;
  4767. }
  4768. };
  4769. template<typename T>
  4770. class tray
  4771. {
  4772. typedef T value_type;
  4773. typedef tray<T> type;
  4774. protected:
  4775. tvec3<T> _origin;
  4776. tvec3<T> _direction;
  4777. public:
  4778. tray():
  4779. _origin(value_type(0),value_type(0),value_type(0)),
  4780. _direction(value_type(0),value_type(0),value_type(1))
  4781. {}
  4782. tray(const tvec3<T>& origin, const tvec3<T>& direction):
  4783. _origin(origin),
  4784. _direction(direction)
  4785. {}
  4786. /**
  4787. * ߵ
  4788. */
  4789. void setOrigin(const tvec3<T>& origin)
  4790. {
  4791. _origin = origin;
  4792. }
  4793. /**
  4794. * ߵ
  4795. */
  4796. const tvec3<T>& getOrigin(void) const
  4797. {
  4798. return _origin;
  4799. }
  4800. /**
  4801. * ߵķ
  4802. */
  4803. void setDirection(const tvec3<T>& dir)
  4804. {
  4805. _direction = dir;
  4806. }
  4807. /**
  4808. * ߵķ
  4809. */
  4810. const tvec3<T>& getDirection(void) const
  4811. {
  4812. return _direction;
  4813. }
  4814. /**
  4815. * Gets the position of a point t units along the ray.
  4816. */
  4817. tvec3<T> getPoint(T time) const
  4818. {
  4819. return tvec3<T>(_origin + (_direction * time));
  4820. }
  4821. /**
  4822. * box
  4823. * ,ֵеfirst == true.false
  4824. * secondΪߵľ
  4825. * getPoint򷵻ؽ
  4826. */
  4827. std::pair<bool, T> intersects(const AxisAlignedBox<T>& box) const
  4828. {
  4829. T lowt = 0.0f;
  4830. T t;
  4831. bool hit = false;
  4832. tvec3<T> hitpoint;
  4833. tvec3<T> min = box.getMinimum();
  4834. tvec3<T> max = box.getMaximum();
  4835. /**
  4836. * ڰΧ
  4837. */
  4838. if ( _origin > min && _origin < max )
  4839. {
  4840. return std::pair<bool, T>(true, 0.0f);
  4841. }
  4842. // Check each face in turn, only check closest 3
  4843. // Min x
  4844. if (_origin.x <= min.x && _direction.x > 0)
  4845. {
  4846. t = (min.x - _origin.x) / _direction.x;
  4847. if (t >= 0)
  4848. {
  4849. // Substitute t back into ray and check bounds and dist
  4850. hitpoint = _origin + _direction * t;
  4851. if (hitpoint.y >= min.y &&
  4852. hitpoint.y <= max.y &&
  4853. hitpoint.z >= min.z &&
  4854. hitpoint.z <= max.z &&
  4855. (!hit || t < lowt))
  4856. {
  4857. hit = true;
  4858. lowt = t;
  4859. }
  4860. }
  4861. }
  4862. // Max x
  4863. if (_origin.x >= max.x && _direction.x < 0)
  4864. {
  4865. t = (max.x - _origin.x) / _direction.x;
  4866. if (t >= 0)
  4867. {
  4868. // Substitute t back into ray and check bounds and dist
  4869. hitpoint = _origin + _direction * t;
  4870. if (hitpoint.y >= min.y &&
  4871. hitpoint.y <= max.y &&
  4872. hitpoint.z >= min.z &&
  4873. hitpoint.z <= max.z &&
  4874. (!hit || t < lowt))
  4875. {
  4876. hit = true;
  4877. lowt = t;
  4878. }
  4879. }
  4880. }
  4881. // Min y
  4882. if (_origin.y <= min.y && _direction.y > 0)
  4883. {
  4884. t = (min.y - _origin.y) / _direction.y;
  4885. if (t >= 0)
  4886. {
  4887. // Substitute t back into ray and check bounds and dist
  4888. hitpoint = _origin + _direction * t;
  4889. if (hitpoint.x >= min.x &&
  4890. hitpoint.x <= max.x &&
  4891. hitpoint.z >= min.z &&
  4892. hitpoint.z <= max.z &&
  4893. (!hit || t < lowt))
  4894. {
  4895. hit = true;
  4896. lowt = t;
  4897. }
  4898. }
  4899. }
  4900. // Max y
  4901. if (_origin.y >= max.y && _direction.y < 0)
  4902. {
  4903. t = (max.y - _origin.y) / _direction.y;
  4904. if (t >= 0)
  4905. {
  4906. // Substitute t back into ray and check bounds and dist
  4907. hitpoint = _origin + _direction * t;
  4908. if (hitpoint.x >= min.x &&
  4909. hitpoint.x <= max.x &&
  4910. hitpoint.z >= min.z &&
  4911. hitpoint.z <= max.z &&
  4912. (!hit || t < lowt))
  4913. {
  4914. hit = true;
  4915. lowt = t;
  4916. }
  4917. }
  4918. }
  4919. // Min z
  4920. if (_origin.z <= min.z && _direction.z > 0)
  4921. {
  4922. t = (min.z - _origin.z) / _direction.z;
  4923. if (t >= 0)
  4924. {
  4925. // Substitute t back into ray and check bounds and dist
  4926. hitpoint = _origin + _direction * t;
  4927. if (hitpoint.x >= min.x &&
  4928. hitpoint.x <= max.x &&
  4929. hitpoint.y >= min.y &&
  4930. hitpoint.y <= max.y &&
  4931. (!hit || t < lowt))
  4932. {
  4933. hit = true;
  4934. lowt = t;
  4935. }
  4936. }
  4937. }
  4938. // Max z
  4939. if (_origin.z >= max.z && _direction.z < 0)
  4940. {
  4941. t = (max.z - _origin.z) / _direction.z;
  4942. if (t >= 0)
  4943. {
  4944. // Substitute t back into ray and check bounds and dist
  4945. hitpoint = _origin + _direction * t;
  4946. if (hitpoint.x >= min.x &&
  4947. hitpoint.x <= max.x &&
  4948. hitpoint.y >= min.y &&
  4949. hitpoint.y <= max.y &&
  4950. (!hit || t < lowt))
  4951. {
  4952. hit = true;
  4953. lowt = t;
  4954. }
  4955. }
  4956. }
  4957. return std::pair<bool, T>(hit, lowt);
  4958. }
  4959. };
  4960. template<class T>
  4961. class Plane
  4962. {
  4963. public:
  4964. tvec3<T> _normal;
  4965. T _distance;
  4966. public:
  4967. Plane ()
  4968. {
  4969. _normal = tvec3<T>(0,0,0);
  4970. _distance = 0.0f;
  4971. }
  4972. Plane (const Plane& right)
  4973. {
  4974. _normal = right._normal;
  4975. _distance = right._distance;
  4976. }
  4977. /** Construct a plane through a normal, and a distance to move the plane along the normal.*/
  4978. Plane (const tvec3<T>& rkNormal, T fConstant)
  4979. {
  4980. _normal = rkNormal;
  4981. _distance = -fConstant;
  4982. }
  4983. /** Construct a plane using the 4 constants directly **/
  4984. Plane (T x, T y, T z, T o)
  4985. {
  4986. _normal = tvec3<T>(x, y, z);
  4987. T invLen = 1.0f / (_normal).length();
  4988. _normal *= invLen;
  4989. _distance = o * invLen;
  4990. }
  4991. Plane (const tvec3<T>& rkNormal, const tvec3<T>& rkPoint)
  4992. {
  4993. redefine(rkNormal, rkPoint);
  4994. }
  4995. Plane (const tvec3<T>& rkPoint0, const tvec3<T>& rkPoint1,const tvec3<T>& rkPoint2)
  4996. {
  4997. redefine(rkPoint0, rkPoint1, rkPoint2);
  4998. }
  4999. /**
  5000. * ľ
  5001. */
  5002. float distance(const tvec3<T> &pos) const
  5003. {
  5004. return dot(_normal,pos) + _distance;
  5005. }
  5006. /** The "positive side" of the plane is the half space to which the
  5007. plane normal points. The "negative side" is the other half
  5008. space. The flag "no side" indicates the plane itself.
  5009. */
  5010. enum Side
  5011. {
  5012. NO_SIDE,
  5013. POSITIVE_SIDE,
  5014. NEGATIVE_SIDE,
  5015. BOTH_SIDE
  5016. };
  5017. Side getSide (const tvec3<T>& rkPoint) const
  5018. {
  5019. float fDistance = getDistance(rkPoint);
  5020. if ( fDistance < 0.0 )
  5021. return Plane::NEGATIVE_SIDE;
  5022. if ( fDistance > 0.0 )
  5023. return Plane::POSITIVE_SIDE;
  5024. return Plane::NO_SIDE;
  5025. }
  5026. Side getSide (const tvec3<T>& centre, const tvec3<T>& halfSize) const
  5027. {
  5028. // Calculate the distance between box centre and the plane
  5029. float dist = getDistance(centre);
  5030. // Calculate the maximise allows absolute distance for
  5031. // the distance between box centre and plane
  5032. float maxAbsDist = _normal.absDot(halfSize);
  5033. if (dist < -maxAbsDist)
  5034. return Plane::NEGATIVE_SIDE;
  5035. if (dist > +maxAbsDist)
  5036. return Plane::POSITIVE_SIDE;
  5037. return Plane::BOTH_SIDE;
  5038. }
  5039. float getDistance (const tvec3<T>& rkPoint) const
  5040. {
  5041. return _normal.dot(rkPoint) + _distance;
  5042. }
  5043. void redefine(const tvec3<T>& rkPoint0, const tvec3<T>& rkPoint1,
  5044. const tvec3<T>& rkPoint2)
  5045. {
  5046. tvec3<T> kEdge1 = rkPoint1 - rkPoint0;
  5047. tvec3<T> kEdge2 = rkPoint2 - rkPoint0;
  5048. _normal = cross(kEdge1,kEdge2);
  5049. _normal.normalise();
  5050. _distance = -dot(_normal,rkPoint0);
  5051. }
  5052. /** Redefine this plane based on a normal and a point. */
  5053. void redefine(const tvec3<T>& rkNormal, const tvec3<T>& rkPoint)
  5054. {
  5055. _normal = rkNormal;
  5056. _distance = -dot(rkNormal,rkPoint);
  5057. }
  5058. // tvec3<T> projectVector(const tvec3<T>& p) const
  5059. // {
  5060. // matrix3 xform;
  5061. // xform[0][0] = 1.0f - _normal.x * _normal.x;
  5062. // xform[0][1] = -_normal.x * _normal.y;
  5063. // xform[0][2] = -_normal.x * _normal.z;
  5064. // xform[1][0] = -_normal.y * _normal.x;
  5065. // xform[1][1] = 1.0f - _normal.y * _normal.y;
  5066. // xform[1][2] = -_normal.y * _normal.z;
  5067. // xform[2][0] = -_normal.z * _normal.x;
  5068. // xform[2][1] = -_normal.z * _normal.y;
  5069. // xform[2][2] = 1.0f - _normal.z * _normal.z;
  5070. // return xform * p;
  5071. // }
  5072. /** Normalises the plane.
  5073. @remarks
  5074. This method normalises the plane's normal and the length scale of d
  5075. is as well.
  5076. @note
  5077. This function will not crash for zero-sized vectors, but there
  5078. will be no changes made to their components.
  5079. @returns The previous length of the plane's normal.
  5080. */
  5081. float normalise(void)
  5082. {
  5083. float fLength = _normal.length();
  5084. // Will also work for zero-sized vectors, but will change nothing
  5085. if (fLength > 1e-08f)
  5086. {
  5087. float fInvLength = 1.0f / fLength;
  5088. _normal *= fInvLength;
  5089. _distance *= fInvLength;
  5090. }
  5091. return fLength;
  5092. }
  5093. /// Comparison operator
  5094. bool operator==(const Plane& right) const
  5095. {
  5096. return (right._distance == _distance && right._normal == _normal);
  5097. }
  5098. bool operator!=(const Plane& right) const
  5099. {
  5100. return (right._distance != _distance && right._normal != _normal);
  5101. }
  5102. };
  5103. template<class T>
  5104. class tfrustum
  5105. {
  5106. public:
  5107. enum
  5108. {
  5109. FRUSTUM_LEFT = 0,
  5110. FRUSTUM_RIGHT = 1,
  5111. FRUSTUM_TOP = 2,
  5112. FRUSTUM_BOTTOM = 3,
  5113. FRUSTUM_FAR = 4,
  5114. FRUSTUM_NEAR = 5,
  5115. };
  5116. public:
  5117. /**
  5118. * project * modleview
  5119. */
  5120. void loadFrustum(const tmat4x4<T> &mvp)
  5121. {
  5122. const T* dataPtr = mvp.data();
  5123. _planes[FRUSTUM_LEFT ] = Plane<T>(dataPtr[12] - dataPtr[0], dataPtr[13] - dataPtr[1], dataPtr[14] - dataPtr[2], dataPtr[15] - dataPtr[3]);
  5124. _planes[FRUSTUM_RIGHT ] = Plane<T>(dataPtr[12] + dataPtr[0], dataPtr[13] + dataPtr[1], dataPtr[14] + dataPtr[2], dataPtr[15] + dataPtr[3]);
  5125. _planes[FRUSTUM_TOP ] = Plane<T>(dataPtr[12] - dataPtr[4], dataPtr[13] - dataPtr[5], dataPtr[14] - dataPtr[6], dataPtr[15] - dataPtr[7]);
  5126. _planes[FRUSTUM_BOTTOM] = Plane<T>(dataPtr[12] + dataPtr[4], dataPtr[13] + dataPtr[5], dataPtr[14] + dataPtr[6], dataPtr[15] + dataPtr[7]);
  5127. _planes[FRUSTUM_FAR ] = Plane<T>(dataPtr[12] - dataPtr[8], dataPtr[13] - dataPtr[9], dataPtr[14] - dataPtr[10], dataPtr[15] - dataPtr[11]);
  5128. _planes[FRUSTUM_NEAR ] = Plane<T>(dataPtr[12] + dataPtr[8], dataPtr[13] + dataPtr[9], dataPtr[14] + dataPtr[10], dataPtr[15] + dataPtr[11]);
  5129. }
  5130. bool pointInFrustum(const tvec3<T> &pos) const
  5131. {
  5132. for (int i = 0; i < 6; i++)
  5133. {
  5134. if (_planes[i].distance(pos) <= 0)
  5135. return false;
  5136. }
  5137. return true;
  5138. }
  5139. bool sphereInFrustum(const tvec3<T> &pos, const float radius) const
  5140. {
  5141. for (int i = 0; i < 6; i++)
  5142. {
  5143. if (_planes[i].distance(pos) <= -radius)
  5144. return false;
  5145. }
  5146. return true;
  5147. }
  5148. bool cubeInFrustum(T minX,T maxX,T minY,T maxY,T minZ,T maxZ) const
  5149. {
  5150. for (int i = 0; i < 6; i++)
  5151. {
  5152. if (_planes[i].distance(tvec3<T>(minX, minY, minZ)) > 0) continue;
  5153. if (_planes[i].distance(tvec3<T>(minX, minY, maxZ)) > 0) continue;
  5154. if (_planes[i].distance(tvec3<T>(minX, maxY, minZ)) > 0) continue;
  5155. if (_planes[i].distance(tvec3<T>(minX, maxY, maxZ)) > 0) continue;
  5156. if (_planes[i].distance(tvec3<T>(maxX, minY, minZ)) > 0) continue;
  5157. if (_planes[i].distance(tvec3<T>(maxX, minY, maxZ)) > 0) continue;
  5158. if (_planes[i].distance(tvec3<T>(maxX, maxY, minZ)) > 0) continue;
  5159. if (_planes[i].distance(tvec3<T>(maxX, maxY, maxZ)) > 0) continue;
  5160. return false;
  5161. }
  5162. return true;
  5163. }
  5164. const Plane<T> &getPlane(const int plane) const
  5165. {
  5166. return _planes[plane];
  5167. }
  5168. protected:
  5169. Plane<T> _planes[6];
  5170. };
  5171. typedef float real;
  5172. typedef tvec2<int> int2;
  5173. typedef tvec2<float> float2;
  5174. typedef tvec2<double> double2;
  5175. typedef tvec2<real> real2;
  5176. typedef tvec3<int> int3;
  5177. typedef tvec3<unsigned> uint3;
  5178. typedef tvec3<float> float3;
  5179. typedef tvec3<double> double3;
  5180. typedef tvec3<real> real3;
  5181. typedef tvec4<int> int4;
  5182. typedef tvec4<float> float4;
  5183. typedef tvec4<double> double4;
  5184. typedef tvec4<real> real4;
  5185. typedef trect<real> rect4;
  5186. typedef trect<int> rect4i;
  5187. typedef AxisAlignedBox<float> aabb3d;
  5188. typedef AxisAlignedBox<real> aabbr;
  5189. typedef AxisAlignedBox2D<float> AABB2D;
  5190. typedef AxisAlignedBox2D<real> aabb2dr;
  5191. typedef AxisAlignedBox2D<int> aabb2di;
  5192. typedef tmat2x2<float> matrix2;
  5193. typedef tmat3x3<float> matrix3;
  5194. typedef tmat4x4<float> matrix4;
  5195. typedef tmat4x4<real> matrix4r;
  5196. typedef tquat<float> quaternion;
  5197. typedef tquat<real> quatr;
  5198. typedef tray<float> Ray;
  5199. typedef tfrustum<float> Frustum;
  5200. typedef tellipsoidModel<float> ellipsoid;
  5201. }