Cry_Vector3.h 40 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. // Description : Common vector class
  9. #ifndef CRYINCLUDE_CRYCOMMON_CRY_VECTOR3_H
  10. #define CRYINCLUDE_CRYCOMMON_CRY_VECTOR3_H
  11. #pragma once
  12. #include "CryEndian.h"
  13. #include <AzCore/Math/Vector3.h>
  14. template<typename T>
  15. struct VecPrecisionValues
  16. {
  17. ILINE static bool CheckGreater(const T value)
  18. {
  19. return value > 0;
  20. }
  21. };
  22. template<>
  23. struct VecPrecisionValues<float>
  24. {
  25. ILINE static bool CheckGreater(const float value)
  26. {
  27. return value > FLT_EPSILON;
  28. }
  29. };
  30. template <typename F>
  31. struct Vec3s_tpl
  32. {
  33. F x, y, z;
  34. ILINE Vec3s_tpl(F vx, F vy, F vz)
  35. : x(vx)
  36. , y(vy)
  37. , z(vz){}
  38. ILINE F& operator [] (int32 index) { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
  39. ILINE F operator [] (int32 index) const { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
  40. };
  41. ///////////////////////////////////////////////////////////////////////////////
  42. ///////////////////////////////////////////////////////////////////////////////
  43. ///////////////////////////////////////////////////////////////////////////////
  44. // class Vec3_tpl
  45. ///////////////////////////////////////////////////////////////////////////////
  46. ///////////////////////////////////////////////////////////////////////////////
  47. ///////////////////////////////////////////////////////////////////////////////
  48. template <typename F>
  49. struct Vec3_tpl
  50. {
  51. typedef F value_type;
  52. enum
  53. {
  54. component_count = 3
  55. };
  56. F x, y, z;
  57. #if defined(_DEBUG)
  58. ILINE Vec3_tpl()
  59. {
  60. if constexpr (sizeof(F) == 4)
  61. {
  62. uint32* p = alias_cast<uint32*>(&x);
  63. p[0] = F32NAN;
  64. p[1] = F32NAN;
  65. p[2] = F32NAN;
  66. }
  67. if constexpr (sizeof(F) == 8)
  68. {
  69. uint64* p = alias_cast<uint64*>(&x);
  70. p[0] = F64NAN;
  71. p[1] = F64NAN;
  72. p[2] = F64NAN;
  73. }
  74. }
  75. #else
  76. ILINE Vec3_tpl() {};
  77. #endif
  78. /*!
  79. * template specialization to initialize a vector
  80. *
  81. * Example:
  82. * Vec3 v0=Vec3(ZERO);
  83. * Vec3 v1=Vec3(MIN);
  84. * Vec3 v2=Vec3(MAX);
  85. */
  86. Vec3_tpl(type_zero)
  87. : x(0)
  88. , y(0)
  89. , z(0) {}
  90. Vec3_tpl(type_min);
  91. Vec3_tpl(type_max);
  92. /*!
  93. * constructors and bracket-operator to initialize a vector
  94. *
  95. * Example:
  96. * Vec3 v0=Vec3(1,2,3);
  97. * Vec3 v1(1,2,3);
  98. * v2.Set(1,2,3);
  99. */
  100. ILINE Vec3_tpl(F vx, F vy, F vz)
  101. : x(vx)
  102. , y(vy)
  103. , z(vz){ assert(this->IsValid()); }
  104. ILINE void operator () (F vx, F vy, F vz) { x = vx; y = vy; z = vz; assert(this->IsValid()); }
  105. ILINE Vec3_tpl<F>& Set(const F xval, const F yval, const F zval) { x = xval; y = yval; z = zval; assert(this->IsValid()); return *this; }
  106. explicit ILINE Vec3_tpl(F f)
  107. : x(f)
  108. , y(f)
  109. , z(f) { assert(this->IsValid()); }
  110. explicit ILINE Vec3_tpl(const AZ::Vector3& v)
  111. {
  112. x = v.GetX();
  113. y = v.GetY();
  114. z = v.GetZ();
  115. }
  116. /*!
  117. * the copy/casting/assignement constructor
  118. *
  119. * Example:
  120. * Vec3 v0=v1;
  121. * Vec3 v0=Vec3(angle);
  122. * Vec3 v0=Vec3(vector4);
  123. */
  124. ILINE Vec3_tpl(const Vec3_tpl& v) { x = v.x; y = v.y; z = v.z; }
  125. template<class F1>
  126. ILINE Vec3_tpl<F>(const Vec3_tpl<F1>&v) {
  127. x = F(v.x);
  128. y = F(v.y);
  129. z = F(v.z);
  130. assert(IsValid());
  131. }
  132. ILINE Vec3_tpl<F>(const Vec2_tpl<F>&v) {
  133. x = v.x;
  134. y = v.y;
  135. z = 0;
  136. assert(IsValid());
  137. }
  138. template<class T>
  139. ILINE Vec3_tpl<F>(const Vec2_tpl<T>&v) {
  140. x = F(v.x);
  141. y = F(v.y);
  142. z = 0;
  143. assert(IsValid());
  144. }
  145. explicit ILINE Vec3_tpl<F>(const Ang3_tpl<F>&v) {
  146. x = v.x;
  147. y = v.y;
  148. z = v.z;
  149. assert(IsValid());
  150. }
  151. template<class T>
  152. explicit ILINE Vec3_tpl<F>(const Ang3_tpl<T>&v) {
  153. x = v.x;
  154. y = v.y;
  155. z = v.z;
  156. assert(IsValid());
  157. }
  158. /*!
  159. * overloaded arithmetic operator
  160. *
  161. * Example:
  162. * Vec3 v0=v1*4;
  163. */
  164. ILINE Vec3_tpl<F> operator * (F k) const
  165. {
  166. const Vec3_tpl<F> v = *this;
  167. return Vec3_tpl<F>(v.x * k, v.y * k, v.z * k);
  168. }
  169. ILINE Vec3_tpl<F> operator / (F k) const
  170. {
  171. k = (F)1.0 / k;
  172. return Vec3_tpl<F>(x * k, y * k, z * k);
  173. }
  174. ILINE friend Vec3_tpl<F> operator * (F f, const Vec3_tpl& vec)
  175. {
  176. return Vec3_tpl((F)(f * vec.x), (F)(f * vec.y), (F)(f * vec.z));
  177. }
  178. ILINE Vec3_tpl<F>& operator *= (F k)
  179. {
  180. x *= k;
  181. y *= k;
  182. z *= k;
  183. return *this;
  184. }
  185. ILINE Vec3_tpl<F>& operator /= (F k)
  186. {
  187. k = (F)1.0 / k;
  188. x *= k;
  189. y *= k;
  190. z *= k;
  191. return *this;
  192. }
  193. ILINE Vec3_tpl<F> operator - (void) const
  194. {
  195. return Vec3_tpl<F>(-x, -y, -z);
  196. }
  197. ILINE Vec3_tpl<F>& Flip()
  198. {
  199. x = -x;
  200. y = -y;
  201. z = -z;
  202. return *this;
  203. }
  204. /*!
  205. * bracked-operator
  206. *
  207. * Example:
  208. * uint32 t=v[0];
  209. */
  210. ILINE F& operator [] (int32 index) { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
  211. ILINE F operator [] (int32 index) const { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
  212. /*!
  213. * functions and operators to compare vector
  214. *
  215. * Example:
  216. * if (v0==v1) dosomething;
  217. */
  218. ILINE bool operator==(const Vec3_tpl<F>& vec)
  219. {
  220. return x == vec.x && y == vec.y && z == vec.z;
  221. }
  222. ILINE bool operator!=(const Vec3_tpl<F>& vec)
  223. {
  224. return !(*this == vec);
  225. }
  226. ILINE friend bool operator ==(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1)
  227. {
  228. return ((v0.x == v1.x) && (v0.y == v1.y) && (v0.z == v1.z));
  229. }
  230. ILINE friend bool operator !=(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1)
  231. {
  232. return !(v0 == v1);
  233. }
  234. ILINE bool IsZero(F e = (F) 0.0) const
  235. {
  236. return (fabs_tpl(x) <= e) && (fabs_tpl(y) <= e) && (fabs_tpl(z) <= e);
  237. }
  238. ILINE bool IsZeroFast(F e = (F) 0.0003) const
  239. {
  240. return (fabs_tpl(x) + fabs_tpl(y) + fabs_tpl(z)) <= e;
  241. }
  242. // Chebyshev distance (axis aligned)
  243. ILINE bool IsEquivalent(const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON) const
  244. {
  245. assert(v1.IsValid());
  246. assert(this->IsValid());
  247. return ((fabs_tpl(x - v1.x) <= epsilon) && (fabs_tpl(y - v1.y) <= epsilon) && (fabs_tpl(z - v1.z) <= epsilon));
  248. }
  249. ILINE static bool IsEquivalent(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON)
  250. {
  251. assert(v0.IsValid());
  252. assert(v1.IsValid());
  253. return ((fabs_tpl(v0.x - v1.x) <= epsilon) && (fabs_tpl(v0.y - v1.y) <= epsilon) && (fabs_tpl(v0.z - v1.z) <= epsilon));
  254. }
  255. // Euclidean distance L2
  256. ILINE bool IsEquivalentL2(const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON) const
  257. {
  258. assert(v1.IsValid());
  259. assert(this->IsValid());
  260. return (*this - v1).GetLengthSquared() <= (epsilon * epsilon);
  261. }
  262. ILINE static bool IsEquivalentL2(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, F epsilon = VEC_EPSILON)
  263. {
  264. assert(v0.IsValid());
  265. assert(v1.IsValid());
  266. return (v0 - v1).GetLengthSquared() <= (epsilon * epsilon);
  267. }
  268. ILINE bool IsUnit(F epsilon = VEC_EPSILON) const
  269. {
  270. return (fabs_tpl(1 - GetLengthSquared()) <= epsilon);
  271. }
  272. bool IsValid() const
  273. {
  274. if (!NumberValid(x))
  275. {
  276. return false;
  277. }
  278. if (!NumberValid(y))
  279. {
  280. return false;
  281. }
  282. if (!NumberValid(z))
  283. {
  284. return false;
  285. }
  286. return true;
  287. }
  288. //! force vector length by normalizing it
  289. ILINE void SetLength(F fLen)
  290. {
  291. F fLenMe = GetLengthSquared();
  292. if (fLenMe < 0.00001f * 0.00001f)
  293. {
  294. return;
  295. }
  296. fLenMe = fLen * isqrt_tpl(fLenMe);
  297. x *= fLenMe;
  298. y *= fLenMe;
  299. z *= fLenMe;
  300. }
  301. ILINE void ClampLength(F maxLength)
  302. {
  303. F sqrLength = GetLengthSquared();
  304. if (sqrLength > (maxLength * maxLength))
  305. {
  306. F scale = maxLength * isqrt_tpl(sqrLength);
  307. x *= scale;
  308. y *= scale;
  309. z *= scale;
  310. }
  311. }
  312. //! calculate the length of the vector
  313. ILINE F GetLength() const
  314. {
  315. return sqrt_tpl(x * x + y * y + z * z);
  316. }
  317. ILINE F GetLengthFloat() const
  318. {
  319. return GetLength();
  320. }
  321. ILINE F GetLengthFast() const
  322. {
  323. return sqrt_fast_tpl(x * x + y * y + z * z);
  324. }
  325. //! calculate the squared length of the vector
  326. ILINE F GetLengthSquared() const
  327. {
  328. return x * x + y * y + z * z;
  329. }
  330. ILINE F GetLengthSquaredFloat() const
  331. {
  332. return GetLengthSquared();
  333. }
  334. //! calculate the length of the vector ignoring the z component
  335. ILINE F GetLength2D() const
  336. {
  337. return sqrt_tpl(x * x + y * y);
  338. }
  339. //! calculate the squared length of the vector ignoring the z component
  340. ILINE F GetLengthSquared2D() const
  341. {
  342. return x * x + y * y;
  343. }
  344. ILINE F GetDistance(const Vec3_tpl<F>& vec1) const
  345. {
  346. return sqrt_tpl((x - vec1.x) * (x - vec1.x) + (y - vec1.y) * (y - vec1.y) + (z - vec1.z) * (z - vec1.z));
  347. }
  348. ILINE F GetSquaredDistance (const Vec3_tpl<F>& v) const
  349. {
  350. return (x - v.x) * (x - v.x) + (y - v.y) * (y - v.y) + (z - v.z) * (z - v.z);
  351. }
  352. ILINE F GetSquaredDistance2D (const Vec3_tpl<F>& v) const
  353. {
  354. return (x - v.x) * (x - v.x) + (y - v.y) * (y - v.y);
  355. }
  356. //! Normalize the vector.
  357. // The default Normalize function is in fact "safe". 0 vectors remain unchanged.
  358. ILINE void Normalize()
  359. {
  360. assert(this->IsValid());
  361. F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z);
  362. x *= fInvLen;
  363. y *= fInvLen;
  364. z *= fInvLen;
  365. }
  366. //! May be faster and less accurate.
  367. ILINE void NormalizeFast()
  368. {
  369. assert(this->IsValid());
  370. F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z);
  371. x *= fInvLen;
  372. y *= fInvLen;
  373. z *= fInvLen;
  374. }
  375. //! Normalize the vector to a scale.
  376. ILINE void Normalize(F scale)
  377. {
  378. assert(this->IsValid());
  379. F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z) * scale;
  380. x *= fInvLen;
  381. y *= fInvLen;
  382. z *= fInvLen;
  383. }
  384. ILINE void NormalizeFast(F scale)
  385. {
  386. assert(this->IsValid());
  387. F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z) * scale;
  388. x *= fInvLen;
  389. y *= fInvLen;
  390. z *= fInvLen;
  391. }
  392. //! Normalize the vector.
  393. // check for null vector - set to the passed in vector (which should be normalised!) if it is null vector
  394. // returns the original length of the vector
  395. ILINE F NormalizeSafe(const struct Vec3_tpl<F>& safe = Vec3_tpl<F>(0, 0, 0))
  396. {
  397. assert(this->IsValid());
  398. F fLen2 = x * x + y * y + z * z;
  399. IF (VecPrecisionValues<F>::CheckGreater(fLen2), 1)
  400. {
  401. F fInvLen = isqrt_tpl(fLen2);
  402. x *= fInvLen;
  403. y *= fInvLen;
  404. z *= fInvLen;
  405. return F(1) / fInvLen;
  406. }
  407. else
  408. {
  409. *this = safe;
  410. return F(0);
  411. }
  412. }
  413. ILINE Vec3_tpl GetNormalizedFloat() const
  414. {
  415. return GetNormalized();
  416. }
  417. //! Return a normalized vector.
  418. ILINE Vec3_tpl GetNormalized() const
  419. {
  420. F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z);
  421. return *this * fInvLen;
  422. }
  423. //! Return a normalized vector.
  424. ILINE Vec3_tpl GetNormalizedFast() const
  425. {
  426. F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z);
  427. return *this * fInvLen;
  428. }
  429. //! Return a safely normalized vector - returns safe vector (should be normalised) if original is zero length.
  430. ILINE Vec3_tpl GetNormalizedSafe(const struct Vec3_tpl<F>& safe = Vec3_tpl<F>(1, 0, 0)) const
  431. {
  432. F fLen2 = x * x + y * y + z * z;
  433. IF (VecPrecisionValues<F>::CheckGreater(fLen2), 1)
  434. {
  435. F fInvLen = isqrt_tpl(fLen2);
  436. return *this * fInvLen;
  437. }
  438. else
  439. {
  440. return safe;
  441. }
  442. }
  443. //! Return a safely normalized vector - returns safe vector (should be normalised) if original is zero length.
  444. ILINE Vec3_tpl GetNormalizedSafeFloat(const struct Vec3_tpl<F>& safe = Vec3_tpl<F>(1, 0, 0)) const
  445. {
  446. return GetNormalizedSafe(safe);
  447. }
  448. //! Return a normalized and scaled vector.
  449. ILINE Vec3_tpl GetNormalized(F scale) const
  450. {
  451. F fInvLen = isqrt_safe_tpl(x * x + y * y + z * z);
  452. return *this * (fInvLen * scale);
  453. }
  454. ILINE Vec3_tpl GetNormalizedFast(F scale) const
  455. {
  456. F fInvLen = isqrt_fast_tpl(x * x + y * y + z * z);
  457. return *this * (fInvLen * scale);
  458. }
  459. //! Permutate coordinates so that z goes to new_z slot.
  460. ILINE Vec3_tpl GetPermutated(int new_z) const { return Vec3_tpl(*(&x + inc_mod3[new_z]), *(&x + dec_mod3[new_z]), *(&x + new_z)); }
  461. //! Returns volume of a box with this vector as diagonal.
  462. ILINE F GetVolume() const { return x * y * z; }
  463. //! Returns a vector that consists of absolute values of this one's coordinates.
  464. ILINE Vec3_tpl<F> abs() const
  465. {
  466. return Vec3_tpl(fabs_tpl(x), fabs_tpl(y), fabs_tpl(z));
  467. }
  468. //! Check for min bounds.
  469. ILINE void CheckMin(const Vec3_tpl<F> other)
  470. {
  471. x = min(other.x, x);
  472. y = min(other.y, y);
  473. z = min(other.z, z);
  474. }
  475. //! Check for max bounds.
  476. ILINE void CheckMax(const Vec3_tpl<F> other)
  477. {
  478. x = max(other.x, x);
  479. y = max(other.y, y);
  480. z = max(other.z, z);
  481. }
  482. /*!
  483. * Sets a vector orthogonal to the input vector.
  484. *
  485. * Example:
  486. * Vec3 v;
  487. * v.SetOrthogonal( i );
  488. */
  489. ILINE void SetOrthogonal(const Vec3_tpl<F>& v)
  490. {
  491. sqr(F(0.9)) * (v | v) - v.x * v.x < 0 ? (x = -v.z, y = 0, z = v.x) : (x = 0, y = v.z, z = -v.y);
  492. }
  493. //! Returns a vector orthogonal to this one.
  494. ILINE Vec3_tpl GetOrthogonal() const
  495. {
  496. return sqr(F(0.9)) * (x * x + y * y + z * z) - x * x < 0 ? Vec3_tpl<F>(-z, 0, x) : Vec3_tpl<F>(0, z, -y);
  497. }
  498. /*!
  499. * Project a point/vector on a (virtual) plane
  500. * Consider we have a plane going through the origin.
  501. * Because d=0 we need just the normal. The vector n is assumed to be a unit-vector.
  502. *
  503. * Example:
  504. * Vec3 result=Vec3::CreateProjection( incident, normal );
  505. */
  506. ILINE void SetProjection(const Vec3_tpl& i, const Vec3_tpl& n)
  507. {
  508. *this = i - n * (n | i);
  509. }
  510. ILINE static Vec3_tpl<F> CreateProjection(const Vec3_tpl& i, const Vec3_tpl& n)
  511. {
  512. return i - n * (n | i);
  513. }
  514. /*!
  515. * Calculate a reflection vector. Vec3 n is assumed to be a unit-vector.
  516. *
  517. * Example:
  518. * Vec3 result=Vec3::CreateReflection( incident, normal );
  519. */
  520. ILINE void SetReflection(const Vec3_tpl<F>& i, const Vec3_tpl<F>& n)
  521. {
  522. *this = (n * (i | n) * 2) - i;
  523. }
  524. ILINE static Vec3_tpl<F> CreateReflection(const Vec3_tpl<F>& i, const Vec3_tpl<F>& n)
  525. {
  526. return (n * (i | n) * 2) - i;
  527. }
  528. /*!
  529. * Linear-Interpolation between Vec3 (lerp)
  530. *
  531. * Example:
  532. * Vec3 r=Vec3::CreateLerp( p, q, 0.345f );
  533. */
  534. ILINE void SetLerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
  535. {
  536. const Vec3_tpl<F> diff = q - p;
  537. *this = p + (diff * t);
  538. }
  539. ILINE static Vec3_tpl<F> CreateLerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
  540. {
  541. const Vec3_tpl<F> diff = q - p;
  542. return p + (diff * t);
  543. }
  544. /*!
  545. * Spherical-Interpolation between 3d-vectors (geometrical slerp)
  546. * both vectors are assumed to be normalized.
  547. *
  548. * Example:
  549. * Vec3 r=Vec3::CreateSlerp( p, q, 0.5674f );
  550. */
  551. void SetSlerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
  552. {
  553. assert(p.IsUnit(0.005f));
  554. assert(q.IsUnit(0.005f));
  555. // calculate cosine using the "inner product" between two vectors: p*q=cos(radiant)
  556. F cosine = clamp_tpl((p | q), F(-1), F(1));
  557. //we explore the special cases where the both vectors are very close together,
  558. //in which case we approximate using the more economical LERP and avoid "divisions by zero" since sin(Angle) = 0 as Angle=0
  559. if (cosine >= (F)0.99)
  560. {
  561. SetLerp(p, q, t); //perform LERP:
  562. Normalize();
  563. }
  564. else
  565. {
  566. //perform SLERP: because of the LERP-check above, a "division by zero" is not possible
  567. F rad = acos_tpl(cosine);
  568. F scale_0 = sin_tpl((1 - t) * rad);
  569. F scale_1 = sin_tpl(t * rad);
  570. *this = (p * scale_0 + q * scale_1) / sin_tpl(rad);
  571. Normalize();
  572. }
  573. }
  574. ILINE static Vec3_tpl<F> CreateSlerp(const Vec3_tpl<F>& p, const Vec3_tpl<F>& q, F t)
  575. {
  576. Vec3_tpl<F> v;
  577. v.SetSlerp(p, q, t);
  578. return v;
  579. }
  580. /*!
  581. * Quadratic-Interpolation between vectors v0,v1,v2.
  582. * This is repeated linear interpolation from 3 points and the resulting curve is a parabola.
  583. * If t is in the range [0...1], then the curve goes only through v0 and v2.
  584. *
  585. * Example:
  586. * Vec3 ip; ip.SetQuadraticCurve( v0,v1,v2, 0.345f );
  587. */
  588. ILINE void SetQuadraticCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t1)
  589. {
  590. F t0 = 1.0f - t1;
  591. *this = t0 * t0 * v0 + t0 * t1 * 2.0f * v1 + t1 * t1 * v2;
  592. }
  593. ILINE static Vec3_tpl<F> CreateQuadraticCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t)
  594. {
  595. Vec3_tpl<F> ip;
  596. ip.SetQuadraticCurve(v0, v1, v2, t);
  597. return ip;
  598. }
  599. /*!
  600. * Cubic-Interpolation between vectors v0,v1,v2,v3.
  601. * This is repeated linear interpolation from 4 points.
  602. * If t is in the range [0...1], then the curve goes only through v0 and v3.
  603. *
  604. * Example:
  605. * Vec3 ip; ip.SetCubicCurve( v0,v1,v2,v3, 0.345f );
  606. */
  607. ILINE void SetCubicCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, const Vec3_tpl<F>& v3, F t1)
  608. {
  609. F t0 = 1.0f - t1;
  610. *this = t0 * t0 * t0 * v0 + 3 * t0 * t0 * t1 * v1 + 3 * t0 * t1 * t1 * v2 + t1 * t1 * t1 * v3;
  611. }
  612. ILINE static Vec3_tpl<F> CreateCubicCurve(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, const Vec3_tpl<F>& v3, F t)
  613. {
  614. Vec3_tpl<F> ip;
  615. ip.SetCubicCurve(v0, v1, v2, v3, t);
  616. return ip;
  617. }
  618. /*!
  619. * Spline-Interpolation between vectors v0,v1,v2.
  620. * This is a variation of a quadratic curve.
  621. * If t is in the range [0...1], then the spline goes through all 3 points
  622. *
  623. * Example:
  624. * Vec3 ip; ip.SetSplineInterpolation( v0,v1,v2, 0.345f );
  625. */
  626. ILINE void SetQuadraticSpline(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t)
  627. {
  628. SetQuadraticCurve(v0, v1 - (v0 * 0.5f + v1 + v2 * 0.5f - v1 * 2.0f), v2, t);
  629. }
  630. ILINE static Vec3_tpl<F> CreateQuadraticSpline(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2, F t)
  631. {
  632. Vec3_tpl<F> ip;
  633. ip.SetQuadraticSpline(v0, v1, v2, t);
  634. return ip;
  635. }
  636. /*!
  637. * Rotate a vector using angle&axis.
  638. *
  639. * Example:
  640. * Vec3 r=v.GetRotated(axis,angle);
  641. */
  642. ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl<F>& axis, F angle) const
  643. {
  644. return GetRotated(axis, cos_tpl(angle), sin_tpl(angle));
  645. }
  646. ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl<F>& axis, F cosa, F sina) const
  647. {
  648. Vec3_tpl<F> zax = axis * (*this | axis);
  649. Vec3_tpl<F> xax = *this - zax;
  650. Vec3_tpl<F> yax = axis % xax;
  651. return xax * cosa + yax * sina + zax;
  652. }
  653. /*!
  654. * Rotate a vector around a center using angle&axis.
  655. *
  656. * Example:
  657. * Vec3 r=v.GetRotated(axis,angle);
  658. */
  659. ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl& center, const Vec3_tpl<F>& axis, F angle) const
  660. {
  661. return center + (*this - center).GetRotated(axis, angle);
  662. }
  663. ILINE Vec3_tpl<F> GetRotated(const Vec3_tpl<F>& center, const Vec3_tpl<F>& axis, F cosa, F sina) const
  664. {
  665. return center + (*this - center).GetRotated(axis, cosa, sina);
  666. }
  667. /*!
  668. * Component wise multiplication of two vectors.
  669. */
  670. ILINE Vec3_tpl CompMul(const Vec3_tpl<F> rhs) const
  671. {
  672. return(Vec3_tpl(x * rhs.x, y * rhs.y, z * rhs.z));
  673. }
  674. //! Three methods for a "dot-product" operation.
  675. ILINE F Dot (const Vec3_tpl<F> v) const
  676. {
  677. return x * v.x + y * v.y + z * v.z;
  678. }
  679. //! Two methods for a "cross-product" operation.
  680. ILINE Vec3_tpl<F> Cross (const Vec3_tpl<F> vec2) const
  681. {
  682. return Vec3_tpl<F>(y * vec2.z - z * vec2.y, z * vec2.x - x * vec2.z, x * vec2.y - y * vec2.x);
  683. }
  684. //f32* fptr=vec;
  685. DEPRECATED operator F* () { return (F*)this; }
  686. template <class T>
  687. explicit DEPRECATED Vec3_tpl(const T* src) { x = F(src[0]); y = F(src[1]); z = F(src[2]); }
  688. ILINE Vec3_tpl& zero() { x = y = z = 0; return *this; }
  689. ILINE F len() const
  690. {
  691. return sqrt_tpl(x * x + y * y + z * z);
  692. }
  693. ILINE F len2() const
  694. {
  695. return x * x + y * y + z * z;
  696. }
  697. ILINE Vec3_tpl& normalize()
  698. {
  699. F len2 = x * x + y * y + z * z;
  700. if (len2 > (F)1e-20f)
  701. {
  702. F rlen = isqrt_tpl(len2);
  703. x *= rlen;
  704. y *= rlen;
  705. z *= rlen;
  706. }
  707. else
  708. {
  709. Set(0, 0, 1);
  710. }
  711. return *this;
  712. }
  713. ILINE Vec3_tpl normalized() const
  714. {
  715. F len2 = x * x + y * y + z * z;
  716. if (len2 > (F)1e-20f)
  717. {
  718. F rlen = isqrt_tpl(len2);
  719. return Vec3_tpl(x * rlen, y * rlen, z * rlen);
  720. }
  721. else
  722. {
  723. return Vec3_tpl(0, 0, 1);
  724. }
  725. }
  726. //vector subtraction
  727. template<class F1>
  728. ILINE Vec3_tpl<F1> sub(const Vec3_tpl<F1>& v) const
  729. {
  730. return Vec3_tpl<F1>(x - v.x, y - v.y, z - v.z);
  731. }
  732. //vector scale
  733. template<class F1>
  734. ILINE Vec3_tpl<F1> scale(const F1 k) const
  735. {
  736. return Vec3_tpl<F>(x * k, y * k, z * k);
  737. }
  738. //vector dot product
  739. template<class F1>
  740. ILINE F1 dot(const Vec3_tpl<F1>& v) const
  741. {
  742. return (F1)(x * v.x + y * v.y + z * v.z);
  743. }
  744. //vector cross product
  745. template<class F1>
  746. ILINE Vec3_tpl<F1> cross(const Vec3_tpl<F1>& v) const
  747. {
  748. return Vec3_tpl<F1>(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
  749. }
  750. };
  751. using Vec3i = Vec3_tpl<int32>;
  752. // dot product (2 versions)
  753. template<class F1, class F2>
  754. ILINE F1 operator * (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  755. {
  756. return v0.Dot(v1);
  757. }
  758. template<class F1, class F2>
  759. ILINE F1 operator | (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  760. {
  761. return v0.Dot(v1);
  762. }
  763. // cross product (2 versions)
  764. template<class F1, class F2>
  765. ILINE Vec3_tpl<F1> operator ^ (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  766. {
  767. return v0.Cross(v1);
  768. }
  769. template<class F1, class F2>
  770. ILINE Vec3_tpl<F1> operator % (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  771. {
  772. return v0.Cross(v1);
  773. }
  774. //vector addition
  775. template<class F1, class F2>
  776. ILINE Vec3_tpl<F1> operator + (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  777. {
  778. return Vec3_tpl<F1>(static_cast<F1>(v0.x + v1.x), static_cast<F1>(v0.y + v1.y), static_cast<F1>(v0.z + v1.z));
  779. }
  780. //vector addition
  781. template<class F1, class F2>
  782. ILINE Vec3_tpl<F1> operator + (const Vec2_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  783. {
  784. return Vec3_tpl<F1>(v0.x + v1.x, v0.y + v1.y, v1.z);
  785. }
  786. //vector addition
  787. template<class F1, class F2>
  788. ILINE Vec3_tpl<F1> operator + (const Vec3_tpl<F1>& v0, const Vec2_tpl<F2>& v1)
  789. {
  790. return Vec3_tpl<F1>(v0.x + v1.x, v0.y + v1.y, v0.z);
  791. }
  792. //vector subtraction
  793. template<class F1, class F2>
  794. ILINE Vec3_tpl<F1> operator - (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  795. {
  796. return Vec3_tpl<F1>((F1)(v0.x - v1.x), (F1)(v0.y - v1.y), (F1)(v0.z - v1.z));
  797. }
  798. template<class F1, class F2>
  799. ILINE Vec3_tpl<F1> operator - (const Vec2_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  800. {
  801. return Vec3_tpl<F1>(v0.x - v1.x, v0.y - v1.y, 0.0f - v1.z);
  802. }
  803. template<class F1, class F2>
  804. ILINE Vec3_tpl<F1> operator - (const Vec3_tpl<F1>& v0, const Vec2_tpl<F2>& v1)
  805. {
  806. return Vec3_tpl<F1>(v0.x - v1.x, v0.y - v1.y, v0.z);
  807. }
  808. //---------------------------------------------------------------------------
  809. //vector self-addition
  810. template<class F1, class F2>
  811. ILINE Vec3_tpl<F1>& operator += (Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  812. {
  813. v0 = v0 + v1;
  814. return v0;
  815. }
  816. //vector self-subtraction
  817. template<class F1, class F2>
  818. ILINE Vec3_tpl<F1>& operator -= (Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  819. {
  820. v0 = v0 - v1;
  821. return v0;
  822. }
  823. template<class F1, class F2>
  824. ILINE Vec3_tpl<F1> operator / (const Vec3_tpl<F1>& v0, const Vec3_tpl<F2>& v1)
  825. {
  826. return Vec3_tpl<F1>(v0.x / v1.x, v0.y / v1.y, v0.z / v1.z);
  827. }
  828. template <class F>
  829. ILINE bool IsEquivalent(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, f32 epsilon = VEC_EPSILON)
  830. {
  831. return ((fabs_tpl(v0.x - v1.x) <= epsilon) && (fabs_tpl(v0.y - v1.y) <= epsilon) && (fabs_tpl(v0.z - v1.z) <= epsilon));
  832. }
  833. ///////////////////////////////////////////////////////////////////////////////
  834. // Typedefs //
  835. ///////////////////////////////////////////////////////////////////////////////
  836. typedef Vec3_tpl<f32> Vec3; // always 32 bit
  837. template<>
  838. inline Vec3_tpl<f32>::Vec3_tpl(type_min) { x = y = z = -3.3E38f; }
  839. template<>
  840. inline Vec3_tpl<f32>::Vec3_tpl(type_max) { x = y = z = 3.3E38f; }
  841. ///////////////////////////////////////////////////////////////////////////////
  842. ///////////////////////////////////////////////////////////////////////////////
  843. ///////////////////////////////////////////////////////////////////////////////
  844. // struct Ang3_tpl
  845. ///////////////////////////////////////////////////////////////////////////////
  846. ///////////////////////////////////////////////////////////////////////////////
  847. ///////////////////////////////////////////////////////////////////////////////
  848. template <typename F>
  849. struct Ang3_tpl
  850. {
  851. F x, y, z;
  852. #if defined(_DEBUG)
  853. ILINE Ang3_tpl()
  854. {
  855. if constexpr (sizeof(F) == 4)
  856. {
  857. uint32* p = alias_cast<uint32*>(&x);
  858. p[0] = F32NAN;
  859. p[1] = F32NAN;
  860. p[2] = F32NAN;
  861. }
  862. if constexpr (sizeof(F) == 8)
  863. {
  864. uint64* p = alias_cast<uint64*>(&x);
  865. p[0] = F64NAN;
  866. p[1] = F64NAN;
  867. p[2] = F64NAN;
  868. }
  869. }
  870. #else
  871. ILINE Ang3_tpl() {};
  872. #endif
  873. Ang3_tpl(type_zero) { x = y = z = 0; }
  874. void operator () (F vx, F vy, F vz) { x = vx; y = vy; z = vz; };
  875. ILINE Ang3_tpl<F>(F vx, F vy, F vz) {
  876. x = vx;
  877. y = vy;
  878. z = vz;
  879. }
  880. explicit ILINE Ang3_tpl(const Vec3_tpl<F>& v)
  881. : x((F)v.x)
  882. , y((F)v.y)
  883. , z((F)v.z) { assert(this->IsValid()); }
  884. ILINE bool operator==(const Ang3_tpl<F>& vec) { return x == vec.x && y == vec.y && z == vec.z; }
  885. ILINE bool operator!=(const Ang3_tpl<F>& vec) { return !(*this == vec); }
  886. ILINE Ang3_tpl<F> operator * (F k) const { return Ang3_tpl<F>(x * k, y * k, z * k); }
  887. ILINE Ang3_tpl<F> operator / (F k) const { k = (F)1.0 / k; return Ang3_tpl<F>(x * k, y * k, z * k); }
  888. ILINE Ang3_tpl<F>& operator *= (F k) { x *= k; y *= k; z *= k; return *this; }
  889. //explicit ILINE Ang3_tpl<F>& operator = (const Vec3_tpl<F>& v) { x=v.x; y=v.y; z=v.z; return *this; }
  890. ILINE Ang3_tpl<F> operator - (void) const { return Ang3_tpl<F>(-x, -y, -z); }
  891. ILINE friend bool operator ==(const Ang3_tpl<F>& v0, const Ang3_tpl<F>& v1)
  892. {
  893. return ((v0.x == v1.x) && (v0.y == v1.y) && (v0.z == v1.z));
  894. }
  895. ILINE void Set(F xval, F yval, F zval) { x = xval; y = yval; z = zval; }
  896. ILINE bool IsEquivalent(const Ang3_tpl<F>& v1, F epsilon = VEC_EPSILON) const
  897. {
  898. return ((fabs_tpl(x - v1.x) <= epsilon) && (fabs_tpl(y - v1.y) <= epsilon) && (fabs_tpl(z - v1.z) <= epsilon));
  899. }
  900. ILINE bool IsInRangePI() const
  901. {
  902. F pi = (F)(gf_PI + 0.001); //we need this to fix fp-precision problem
  903. return ((x > -pi) && (x < pi) && (y > -pi) && (y < pi) && (z > -pi) && (z < pi));
  904. }
  905. //! Normalize angle to -pi and +pi range.
  906. ILINE void RangePI()
  907. {
  908. const F modX = fmod(x + gf_PI, gf_PI2);
  909. x = if_neg_else(modX, modX + gf_PI, modX - gf_PI);
  910. const F modY = fmod(y + gf_PI, gf_PI2);
  911. y = if_neg_else(modY, modY + gf_PI, modY - gf_PI);
  912. const F modZ = fmod(z + gf_PI, gf_PI2);
  913. z = if_neg_else(modZ, modZ + gf_PI, modZ - gf_PI);
  914. }
  915. //! Convert unit Quat to Euler Angles (xyz).
  916. template<class F1>
  917. explicit Ang3_tpl(const Quat_tpl<F1>& q)
  918. {
  919. assert(q.IsValid());
  920. y = F(asin_tpl(max((F)-1.0, min((F)1.0, -(q.v.x * q.v.z - q.w * q.v.y) * 2))));
  921. if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
  922. {
  923. x = F(0);
  924. z = F(atan2_tpl(-2 * (q.v.x * q.v.y - q.w * q.v.z), 1 - (q.v.x * q.v.x + q.v.z * q.v.z) * 2));
  925. }
  926. else
  927. {
  928. x = F(atan2_tpl((q.v.y * q.v.z + q.w * q.v.x) * 2, 1 - (q.v.x * q.v.x + q.v.y * q.v.y) * 2));
  929. z = F(atan2_tpl((q.v.x * q.v.y + q.w * q.v.z) * 2, 1 - (q.v.z * q.v.z + q.v.y * q.v.y) * 2));
  930. }
  931. }
  932. //! Convert Matrix33 to Euler Angles (xyz).
  933. template<class F1>
  934. explicit Ang3_tpl(const Matrix33_tpl<F1>& m)
  935. {
  936. assert(m.IsOrthonormalRH(0.001f));
  937. y = (F)asin_tpl(max((F)-1.0, min((F)1.0, -m.m20)));
  938. if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
  939. {
  940. x = F(0);
  941. z = F(atan2_tpl(-m.m01, m.m11));
  942. }
  943. else
  944. {
  945. x = F(atan2_tpl(m.m21, m.m22));
  946. z = F(atan2_tpl(m.m10, m.m00));
  947. }
  948. }
  949. //! Convert Matrix34 to Euler Angles (xyz).
  950. template<class F1>
  951. explicit Ang3_tpl(const Matrix34_tpl<F1>& m)
  952. {
  953. assert(m.IsOrthonormalRH(0.001f));
  954. y = F(asin_tpl(max((F)-1.0, min((F)1.0, -m.m20))));
  955. if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
  956. {
  957. x = F(0);
  958. z = F(atan2_tpl(-m.m01, m.m11));
  959. }
  960. else
  961. {
  962. x = F(atan2_tpl(m.m21, m.m22));
  963. z = F(atan2_tpl(m.m10, m.m00));
  964. }
  965. }
  966. //! Convert Matrix44 to Euler Angles (xyz).
  967. template<class F1>
  968. explicit Ang3_tpl(const Matrix44_tpl<F1>& m)
  969. {
  970. assert(Matrix33(m).IsOrthonormalRH(0.001f));
  971. y = F(asin_tpl(max((F)-1.0, min((F)1.0, -m.m20))));
  972. if (fabs_tpl(fabs_tpl(y) - (F)((F)g_PI * (F)0.5)) < (F)0.01)
  973. {
  974. x = F(0);
  975. z = F(atan2_tpl(-m.m01, m.m11));
  976. }
  977. else
  978. {
  979. x = F(atan2_tpl(m.m21, m.m22));
  980. z = F(atan2_tpl(m.m10, m.m00));
  981. }
  982. }
  983. template<typename F1>
  984. static ILINE F CreateRadZ(const Vec2_tpl<F1>& v0, const Vec2_tpl<F1>& v1)
  985. {
  986. F cz = v0.x * v1.y - v0.y * v1.x;
  987. F c = v0.x * v1.x + v0.y * v1.y;
  988. return F(atan2_tpl(cz, c));
  989. }
  990. template<typename F1>
  991. static ILINE F CreateRadZ(const Vec3_tpl<F1>& v0, const Vec3_tpl<F1>& v1)
  992. {
  993. F cz = v0.x * v1.y - v0.y * v1.x;
  994. F c = v0.x * v1.x + v0.y * v1.y;
  995. return F(atan2_tpl(cz, c));
  996. }
  997. template<typename F1>
  998. ILINE static Ang3_tpl<F> GetAnglesXYZ(const Quat_tpl<F1>& q) { return Ang3_tpl<F>(q); }
  999. template<typename F1>
  1000. ILINE void SetAnglesXYZ(const Quat_tpl<F1>& q) { *this = Ang3_tpl<F>(q); }
  1001. template<typename F1>
  1002. ILINE static Ang3_tpl<F> GetAnglesXYZ(const Matrix33_tpl<F1>& m) { return Ang3_tpl<F>(m); }
  1003. template<typename F1>
  1004. ILINE void SetAnglesXYZ(const Matrix33_tpl<F1>& m) { *this = Ang3_tpl<F>(m); }
  1005. template<typename F1>
  1006. ILINE static Ang3_tpl<F> GetAnglesXYZ(const Matrix34_tpl<F1>& m) { return Ang3_tpl<F>(m); }
  1007. template<typename F1>
  1008. ILINE void SetAnglesXYZ(const Matrix34_tpl<F1>& m) { *this = Ang3_tpl<F>(m); }
  1009. //---------------------------------------------------------------
  1010. ILINE F& operator [] (int index) { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
  1011. ILINE F operator [] (int index) const { assert(index >= 0 && index <= 2); return ((F*)this)[index]; }
  1012. bool IsValid() const
  1013. {
  1014. if (!NumberValid(x))
  1015. {
  1016. return false;
  1017. }
  1018. if (!NumberValid(y))
  1019. {
  1020. return false;
  1021. }
  1022. if (!NumberValid(z))
  1023. {
  1024. return false;
  1025. }
  1026. return true;
  1027. }
  1028. };
  1029. typedef Ang3_tpl<f32> Ang3;
  1030. //---------------------------------------
  1031. //vector addition
  1032. template<class F1, class F2>
  1033. ILINE Ang3_tpl<F1> operator + (const Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
  1034. {
  1035. return Ang3_tpl<F1>(v0.x + v1.x, v0.y + v1.y, v0.z + v1.z);
  1036. }
  1037. //vector subtraction
  1038. template<class F1, class F2>
  1039. ILINE Ang3_tpl<F1> operator - (const Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
  1040. {
  1041. return Ang3_tpl<F1>(v0.x - v1.x, v0.y - v1.y, v0.z - v1.z);
  1042. }
  1043. //---------------------------------------
  1044. //vector self-addition
  1045. template<class F1, class F2>
  1046. ILINE Ang3_tpl<F1>& operator += (Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
  1047. {
  1048. v0 = v0 + v1;
  1049. return v0;
  1050. }
  1051. //vector self-subtraction
  1052. template<class F1, class F2>
  1053. ILINE Ang3_tpl<F1>& operator -= (Ang3_tpl<F1>& v0, const Ang3_tpl<F2>& v1)
  1054. {
  1055. v0 = v0 - v1;
  1056. return v0;
  1057. }
  1058. ///////////////////////////////////////////////////////////////////////////////
  1059. ///////////////////////////////////////////////////////////////////////////////
  1060. ///////////////////////////////////////////////////////////////////////////////
  1061. // struct CAngleAxis
  1062. ///////////////////////////////////////////////////////////////////////////////
  1063. ///////////////////////////////////////////////////////////////////////////////
  1064. ///////////////////////////////////////////////////////////////////////////////
  1065. template <typename F>
  1066. struct AngleAxis_tpl
  1067. {
  1068. //! storage for the Angle&Axis coordinates.
  1069. F angle;
  1070. Vec3_tpl<F> axis;
  1071. // default quaternion constructor
  1072. AngleAxis_tpl(void) { };
  1073. AngleAxis_tpl(F a, F ax, F ay, F az) { angle = a; axis.x = ax; axis.y = ay; axis.z = az; }
  1074. AngleAxis_tpl(F a, const Vec3_tpl<F>& n) { angle = a; axis = n; }
  1075. void operator () (F a, const Vec3_tpl<F>& n) { angle = a; axis = n; }
  1076. AngleAxis_tpl(const AngleAxis_tpl<F>& aa); //CAngleAxis aa=angleaxis
  1077. const Vec3_tpl<F> operator * (const Vec3_tpl<F>& v) const;
  1078. AngleAxis_tpl(const Quat_tpl<F>& q)
  1079. {
  1080. angle = acos_tpl(q.w) * 2;
  1081. axis = q.v;
  1082. axis.Normalize();
  1083. F s = sin_tpl(angle * (F)0.5);
  1084. if (s == 0)
  1085. {
  1086. angle = 0;
  1087. axis.x = 0;
  1088. axis.y = 0;
  1089. axis.z = 1;
  1090. }
  1091. }
  1092. };
  1093. typedef AngleAxis_tpl<f32> AngleAxis;
  1094. template<typename F>
  1095. ILINE const Vec3_tpl<F> AngleAxis_tpl<F>::operator * (const Vec3_tpl<F>& v) const
  1096. {
  1097. Vec3_tpl<F> origin = axis * (axis | v);
  1098. return origin + (v - origin) * cos_tpl(angle) + (axis % v) * sin_tpl(angle);
  1099. }
  1100. //////////////////////////////////////////////////////////////////////
  1101. template<typename F>
  1102. struct Plane_tpl
  1103. {
  1104. //plane-equation: n.x*x + n.y*y + n.z*z + d > 0 is in front of the plane
  1105. Vec3_tpl<F> n; //!< normal
  1106. F d; //!< distance
  1107. //----------------------------------------------------------------
  1108. #if defined(_DEBUG)
  1109. ILINE Plane_tpl()
  1110. {
  1111. if constexpr (sizeof(F) == 4)
  1112. {
  1113. uint32* p = alias_cast<uint32*>(&n.x);
  1114. p[0] = F32NAN;
  1115. p[1] = F32NAN;
  1116. p[2] = F32NAN;
  1117. p[3] = F32NAN;
  1118. }
  1119. if constexpr (sizeof(F) == 8)
  1120. {
  1121. uint64* p = alias_cast<uint64*>(&n.x);
  1122. p[0] = F64NAN;
  1123. p[1] = F64NAN;
  1124. p[2] = F64NAN;
  1125. p[3] = F64NAN;
  1126. }
  1127. }
  1128. #else
  1129. ILINE Plane_tpl() {};
  1130. #endif
  1131. ILINE Plane_tpl(const Plane_tpl<F>& p) { n = p.n; d = p.d; }
  1132. ILINE Plane_tpl(const Vec3_tpl<F>& normal, const F& distance) { n = normal; d = distance; }
  1133. //! set normal and dist for this plane and then calculate plane type
  1134. ILINE void Set(const Vec3_tpl<F>& vNormal, const F fDist)
  1135. {
  1136. n = vNormal;
  1137. d = fDist;
  1138. }
  1139. ILINE void SetPlane(const Vec3_tpl<F>& normal, const Vec3_tpl<F>& point)
  1140. {
  1141. n = normal;
  1142. d = -(point | normal);
  1143. }
  1144. ILINE static Plane_tpl<F> CreatePlane(const Vec3_tpl<F>& normal, const Vec3_tpl<F>& point)
  1145. {
  1146. return Plane_tpl<F>(normal, -(point | normal));
  1147. }
  1148. ILINE Plane_tpl<F> operator - (void) const { return Plane_tpl<F>(-n, -d); }
  1149. /*!
  1150. * Constructs the plane by tree given Vec3s (=triangle) with a right-hand (anti-clockwise) winding
  1151. *
  1152. * Example 1:
  1153. * Vec3 v0(1,2,3),v1(4,5,6),v2(6,5,6);
  1154. * Plane_tpl<F> plane;
  1155. * plane.SetPlane(v0,v1,v2);
  1156. *
  1157. * Example 2:
  1158. * Vec3 v0(1,2,3),v1(4,5,6),v2(6,5,6);
  1159. * Plane_tpl<F> plane=Plane_tpl<F>::CreatePlane(v0,v1,v2);
  1160. */
  1161. ILINE void SetPlane(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2)
  1162. {
  1163. n = ((v1 - v0) % (v2 - v0)).GetNormalized(); //vector cross-product
  1164. d = -(n | v0); //calculate d-value
  1165. }
  1166. ILINE static Plane_tpl<F> CreatePlane(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, const Vec3_tpl<F>& v2)
  1167. {
  1168. Plane_tpl<F> p;
  1169. p.SetPlane(v0, v1, v2);
  1170. return p;
  1171. }
  1172. /*!
  1173. * Computes signed distance from point to plane.
  1174. * This is the standard plane-equation: d=Ax*By*Cz+D.
  1175. * The normal-vector is assumed to be normalized.
  1176. *
  1177. * Example:
  1178. * Vec3 v(1,2,3);
  1179. * Plane_tpl<F> plane=CalculatePlane(v0,v1,v2);
  1180. * f32 distance = plane|v;
  1181. */
  1182. ILINE F operator | (const Vec3_tpl<F>& point) const { return (n | point) + d; }
  1183. ILINE F DistFromPlane(const Vec3_tpl<F>& vPoint) const { return (n * vPoint + d); }
  1184. ILINE Plane_tpl<F> operator - (const Plane_tpl<F>& p) const { return Plane_tpl<F>(n - p.n, d - p.d); }
  1185. ILINE Plane_tpl<F> operator + (const Plane_tpl<F>& p) const { return Plane_tpl<F>(n + p.n, d + p.d); }
  1186. ILINE void operator -= (const Plane_tpl<F>& p) { d -= p.d; n -= p.n; }
  1187. ILINE Plane_tpl<F> operator * (F s) const { return Plane_tpl<F>(n * s, d * s); }
  1188. ILINE Plane_tpl<F> operator / (F s) const { return Plane_tpl<F>(n / s, d / s); }
  1189. //! check for equality between two planes
  1190. friend bool operator ==(const Plane_tpl<F>& p1, const Plane_tpl<F>& p2)
  1191. {
  1192. if (fabsf(p1.n.x - p2.n.x) > 0.0001f)
  1193. {
  1194. return (false);
  1195. }
  1196. if (fabsf(p1.n.y - p2.n.y) > 0.0001f)
  1197. {
  1198. return (false);
  1199. }
  1200. if (fabsf(p1.n.z - p2.n.z) > 0.0001f)
  1201. {
  1202. return (false);
  1203. }
  1204. if (fabsf(p1.d - p2.d) < 0.01f)
  1205. {
  1206. return(true);
  1207. }
  1208. return (false);
  1209. }
  1210. Vec3_tpl<F> MirrorVector(const Vec3_tpl<F>& i) { return n * (2 * (n | i)) - i; }
  1211. Vec3_tpl<F> MirrorPosition(const Vec3_tpl<F>& i) { return i - n * (2 * ((n | i) + d)); }
  1212. ILINE bool IsValid() const { return !n.IsZeroFast(); } //A plane with a zero normal isn't valid.
  1213. };
  1214. typedef Plane_tpl<f32> Plane; //always 32 bit
  1215. // declare common constants. Must be done after the class for compiler conformance
  1216. // (msvc and clang handle instantiation differently)
  1217. const Vec3_tpl<float> Vec3_Zero(0, 0, 0);
  1218. const Vec3_tpl<float> Vec3_OneX(1, 0, 0);
  1219. const Vec3_tpl<float> Vec3_OneY(0, 1, 0);
  1220. const Vec3_tpl<float> Vec3_OneZ(0, 0, 1);
  1221. const Vec3_tpl<float> Vec3_One(1, 1, 1);
  1222. namespace AZ
  1223. {
  1224. AZ_TYPE_INFO_SPECIALIZE(Vec3, "{DFA993FB-4E92-4A13-BDB3-4E9285A5346F}");
  1225. }
  1226. #endif // CRYINCLUDE_CRYCOMMON_CRY_VECTOR3_H