libs/math/vector.h

   1 /*
   2 Copyright (C) 2001-2006, William Joseph.
   3 All Rights Reserved.
   4
   5 This file is part of GtkRadiant.
   6
   7 GtkRadiant is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 GtkRadiant is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with GtkRadiant; if not, write to the Free Software
  19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20 */
  21
  22 #if !defined(INCLUDED_MATH_VECTOR_H)
  23 #define INCLUDED_MATH_VECTOR_H
  24
  25 /// \file
  26 /// \brief Vector data types and related operations.
  27
  28 #if 0
  29
  30 #define lrint(dbl)              ((int)((dbl) + 0.5))
  31 #define lrintf(flt)             ((int)((flt) + 0.5))
  32
  33 #endif
  34
  35 #if defined (_MSC_VER)
  36
  37 inline int lrint (double flt)
  38 {
  39   int i;
  40
  41         _asm
  42         {
  43     fld flt
  44                 fistp i
  45   };
  46
  47         return i;
  48 }
  49
  50 #else // lrint is part of ISO C99
  51
  52 #define _ISOC9X_SOURCE  1
  53 #define _ISOC99_SOURCE  1
  54
  55 #define __USE_ISOC9X    1
  56 #define __USE_ISOC99    1
  57
  58 #endif
  59
  60 #include <cmath>
  61 #include <cstddef>
  62 #include <float.h>
  63 #include <algorithm>
  64
  65 //#include "debugging/debugging.h"
  66
  67 /// \brief Returns true if \p self is equal to other \p other within \p epsilon.
  68 template<typename Element, typename OtherElement>
  69 inline bool float_equal_epsilon(const Element& self, const OtherElement& other, const Element& epsilon)
  70 {
  71   return fabs(other - self) < epsilon;
  72 }
  73
  74 /// \brief Returns the value midway between \p self and \p other.
  75 template<typename Element>
  76 inline Element float_mid(const Element& self, const Element& other)
  77 {
  78   return Element((self + other) * 0.5);
  79 }
  80
  81 /// \brief Returns \p f rounded to the nearest integer. Note that this is not the same behaviour as casting from float to int.
  82 template<typename Element>
  83 inline int float_to_integer(const Element& f)
  84 {
  85   return lrint(f);
  86 }
  87
  88 /// \brief Returns \p f rounded to the nearest multiple of \p snap.
  89 template<typename Element, typename OtherElement>
  90 inline Element float_snapped(const Element& f, const OtherElement& snap)
  91 {
  92   return Element(float_to_integer(f / snap) * snap);
  93 }
  94
  95 /// \brief Returns true if \p f has no decimal fraction part.
  96 template<typename Element>
  97 inline bool float_is_integer(const Element& f)
  98 {
  99   return f == Element(float_to_integer(f));
 100 }
 101
 102 /// \brief Returns \p self modulated by the range [0, \p modulus)
 103 /// \p self must be in the range [\p -modulus, \p modulus)
 104 template<typename Element, typename ModulusElement>
 105 inline Element float_mod_range(const Element& self, const ModulusElement& modulus)
 106 {
 107   return Element((self < 0.0) ? self + modulus : self);
 108 }
 109
 110 /// \brief Returns \p self modulated by the range [0, \p modulus)
 111 template<typename Element, typename ModulusElement>
 112 inline Element float_mod(const Element& self, const ModulusElement& modulus)
 113 {
 114   return float_mod_range(Element(fmod(static_cast<double>(self), static_cast<double>(modulus))), modulus);
 115 }
 116
 117
 118 template<typename Element>
 119 class BasicVector2
 120 {
 121   Element m_elements[2];
 122 public:
 123   BasicVector2()
 124   {
 125   }
 126   BasicVector2(const Element& x_, const Element& y_)
 127   {
 128     x() = x_;
 129     y() = y_;
 130   }
 131
 132   Element& x()
 133   {
 134     return m_elements[0];
 135   }
 136   const Element& x() const
 137   {
 138     return m_elements[0];
 139   }
 140   Element& y()
 141   {
 142     return m_elements[1];
 143   }
 144   const Element& y() const
 145   {
 146     return m_elements[1];
 147   }
 148
 149   const Element& operator[](std::size_t i) const
 150   {
 151     return m_elements[i];
 152   }
 153   Element& operator[](std::size_t i)
 154   {
 155     return m_elements[i];
 156   }
 157 };
 158
 159
 160 template<typename Element, typename OtherElement>
 161 inline BasicVector2<Element> vector2_added(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 162 {
 163   return BasicVector2<Element>(
 164     Element(self.x() + other.x()),
 165     Element(self.y() + other.y())
 166   );
 167 }
 168 template<typename Element, typename OtherElement>
 169 inline BasicVector2<Element> operator+(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 170 {
 171   return vector2_added(self, other);
 172 }
 173 template<typename Element, typename OtherElement>
 174 inline void vector2_add(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 175 {
 176   self.x() += Element(other.x());
 177   self.y() += Element(other.y());
 178 }
 179 template<typename Element, typename OtherElement>
 180 inline void operator+=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 181 {
 182   vector2_add(self, other);
 183 }
 184
 185
 186 template<typename Element, typename OtherElement>
 187 inline BasicVector2<Element> vector2_subtracted(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 188 {
 189   return BasicVector2<Element>(
 190     Element(self.x() - other.x()),
 191     Element(self.y() - other.y())
 192   );
 193 }
 194 template<typename Element, typename OtherElement>
 195 inline BasicVector2<Element> operator-(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 196 {
 197   return vector2_subtracted(self, other);
 198 }
 199 template<typename Element, typename OtherElement>
 200 inline void vector2_subtract(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 201 {
 202   self.x() -= Element(other.x());
 203   self.y() -= lement(other.y());
 204 }
 205 template<typename Element, typename OtherElement>
 206 inline void operator-=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 207 {
 208   vector2_subtract(self, other);
 209 }
 210
 211
 212 template<typename Element, typename OtherElement>
 213 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, OtherElement other)
 214 {
 215   return BasicVector2<Element>(
 216     Element(self.x() * other),
 217     Element(self.y() * other)
 218   );
 219 }
 220 template<typename Element, typename OtherElement>
 221 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, OtherElement other)
 222 {
 223   return vector2_scaled(self, other);
 224 }
 225 template<typename Element, typename OtherElement>
 226 inline void vector2_scale(BasicVector2<Element>& self, OtherElement other)
 227 {
 228   self.x() *= Element(other);
 229   self.y() *= Element(other);
 230 }
 231 template<typename Element, typename OtherElement>
 232 inline void operator*=(BasicVector2<Element>& self, OtherElement other)
 233 {
 234   vector2_scale(self, other);
 235 }
 236
 237
 238 template<typename Element, typename OtherElement>
 239 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 240 {
 241   return BasicVector2<Element>(
 242     Element(self.x() * other.x()),
 243     Element(self.y() * other.y())
 244   );
 245 }
 246 template<typename Element, typename OtherElement>
 247 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 248 {
 249   return vector2_scaled(self, other);
 250 }
 251 template<typename Element, typename OtherElement>
 252 inline void vector2_scale(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 253 {
 254   self.x() *= Element(other.x());
 255   self.y() *= Element(other.y());
 256 }
 257 template<typename Element, typename OtherElement>
 258 inline void operator*=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 259 {
 260   vector2_scale(self, other);
 261 }
 262
 263 template<typename Element, typename OtherElement>
 264 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 265 {
 266   return BasicVector2<Element>(
 267     Element(self.x() / other.x()),
 268     Element(self.y() / other.y())
 269   );
 270 }
 271 template<typename Element, typename OtherElement>
 272 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 273 {
 274   return vector2_divided(self, other);
 275 }
 276 template<typename Element, typename OtherElement>
 277 inline void vector2_divide(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 278 {
 279   self.x() /= Element(other.x());
 280   self.y() /= Element(other.y());
 281 }
 282 template<typename Element, typename OtherElement>
 283 inline void operator/=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 284 {
 285   vector2_divide(self, other);
 286 }
 287
 288
 289 template<typename Element, typename OtherElement>
 290 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, OtherElement other)
 291 {
 292   return BasicVector2<Element>(
 293     Element(self.x() / other),
 294     Element(self.y() / other)
 295   );
 296 }
 297 template<typename Element, typename OtherElement>
 298 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, OtherElement other)
 299 {
 300   return vector2_divided(self, other);
 301 }
 302 template<typename Element, typename OtherElement>
 303 inline void vector2_divide(BasicVector2<Element>& self, OtherElement other)
 304 {
 305   self.x() /= Element(other);
 306   self.y() /= Element(other);
 307 }
 308 template<typename Element, typename OtherElement>
 309 inline void operator/=(BasicVector2<Element>& self, OtherElement other)
 310 {
 311   vector2_divide(self, other);
 312 }
 313
 314 template<typename Element, typename OtherElement>
 315 inline double vector2_dot(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 316 {
 317   return self.x() * other.x() + self.y() * other.y();
 318 }
 319
 320 template<typename Element>
 321 inline double vector2_length_squared(const BasicVector2<Element>& self)
 322 {
 323   return vector2_dot(self, self);
 324 }
 325
 326 template<typename Element>
 327 inline double vector2_length(const BasicVector2<Element>& self)
 328 {
 329   return sqrt(vector2_length_squared(self));
 330 }
 331
 332 template<typename Element, typename OtherElement>
 333 inline double vector2_cross(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 334 {
 335   return self.x() * other.y() - self.y() * other.x();
 336 }
 337
 338 typedef BasicVector2<float> Vector2;
 339
 340 /// \brief A 3-element vector.
 341 template<typename Element>
 342 class BasicVector3
 343 {
 344   Element m_elements[3];
 345 public:
 346
 347   BasicVector3()
 348   {
 349   }
 350   template<typename OtherElement>
 351   BasicVector3(const BasicVector3<OtherElement>& other)
 352   {
 353     x() = static_cast<Element>(other.x());
 354     y() = static_cast<Element>(other.y());
 355     z() = static_cast<Element>(other.z());
 356   }
 357   BasicVector3(const Element& x_, const Element& y_, const Element& z_)
 358   {
 359     x() = x_;
 360     y() = y_;
 361     z() = z_;
 362   }
 363
 364   Element& x()
 365   {
 366     return m_elements[0];
 367   }
 368   const Element& x() const
 369   {
 370     return m_elements[0];
 371   }
 372   Element& y()
 373   {
 374     return m_elements[1];
 375   }
 376   const Element& y() const
 377   {
 378     return m_elements[1];
 379   }
 380   Element& z()
 381   {
 382     return m_elements[2];
 383   }
 384   const Element& z() const
 385   {
 386     return m_elements[2];
 387   }
 388
 389   const Element& operator[](std::size_t i) const
 390   {
 391     return m_elements[i];
 392   }
 393   Element& operator[](std::size_t i)
 394   {
 395     return m_elements[i];
 396   }
 397
 398   operator BasicVector2<Element>&()
 399   {
 400     return reinterpret_cast<BasicVector2<Element>&>(*this);
 401   }
 402   operator const BasicVector2<Element>&() const
 403   {
 404     return reinterpret_cast<const BasicVector2<Element>&>(*this);
 405   }
 406 };
 407
 408 /// \brief A 3-element vector stored in single-precision floating-point.
 409 typedef BasicVector3<float> Vector3;
 410
 411 const Vector3 g_vector3_identity(0, 0, 0);
 412 const Vector3 g_vector3_max = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
 413 const Vector3 g_vector3_axis_x(1, 0, 0);
 414 const Vector3 g_vector3_axis_y(0, 1, 0);
 415 const Vector3 g_vector3_axis_z(0, 0, 1);
 416
 417 const Vector3 g_vector3_axes[3] = { g_vector3_axis_x, g_vector3_axis_y, g_vector3_axis_z };
 418
 419 inline Vector3& vector3_from_array(float* array)
 420 {
 421   return *reinterpret_cast<Vector3*>(array);
 422 }
 423 inline const Vector3& vector3_from_array(const float* array)
 424 {
 425   return *reinterpret_cast<const Vector3*>(array);
 426 }
 427
 428 template<typename Element>
 429 inline Element* vector3_to_array(BasicVector3<Element>& self)
 430 {
 431   return reinterpret_cast<Element*>(&self);
 432 }
 433 template<typename Element>
 434 inline const Element* vector3_to_array(const BasicVector3<Element>& self)
 435 {
 436   return reinterpret_cast<const Element*>(&self);
 437 }
 438
 439 template<typename Element, typename OtherElement>
 440 inline void vector3_swap(BasicVector3<Element>& self, BasicVector3<OtherElement>& other)
 441 {
 442   std::swap(self.x(), other.x());
 443   std::swap(self.y(), other.y());
 444   std::swap(self.z(), other.z());
 445 }
 446
 447 template<typename Element, typename OtherElement>
 448 inline bool vector3_equal(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 449 {
 450   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z();
 451 }
 452 template<typename Element, typename OtherElement>
 453 inline bool operator==(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 454 {
 455   return vector3_equal(self, other);
 456 }
 457 template<typename Element, typename OtherElement>
 458 inline bool operator!=(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 459 {
 460   return !vector3_equal(self, other);
 461 }
 462
 463
 464 template<typename Element, typename OtherElement, typename Epsilon>
 465 inline bool vector3_equal_epsilon(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other, Epsilon epsilon)
 466 {
 467   return float_equal_epsilon(self.x(), other.x(), epsilon)
 468     && float_equal_epsilon(self.y(), other.y(), epsilon)
 469     && float_equal_epsilon(self.z(), other.z(), epsilon);
 470 }
 471
 472
 473
 474 template<typename Element, typename OtherElement>
 475 inline BasicVector3<Element> vector3_added(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 476 {
 477   return BasicVector3<Element>(
 478     Element(self.x() + other.x()),
 479     Element(self.y() + other.y()),
 480     Element(self.z() + other.z())
 481   );
 482 }
 483 template<typename Element, typename OtherElement>
 484 inline BasicVector3<Element> operator+(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 485 {
 486   return vector3_added(self, other);
 487 }
 488 template<typename Element, typename OtherElement>
 489 inline void vector3_add(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 490 {
 491   self.x() += static_cast<Element>(other.x());
 492   self.y() += static_cast<Element>(other.y());
 493   self.z() += static_cast<Element>(other.z());
 494 }
 495 template<typename Element, typename OtherElement>
 496 inline void operator+=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 497 {
 498   vector3_add(self, other);
 499 }
 500
 501 template<typename Element, typename OtherElement>
 502 inline BasicVector3<Element> vector3_subtracted(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 503 {
 504   return BasicVector3<Element>(
 505     Element(self.x() - other.x()),
 506     Element(self.y() - other.y()),
 507     Element(self.z() - other.z())
 508   );
 509 }
 510 template<typename Element, typename OtherElement>
 511 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 512 {
 513   return vector3_subtracted(self, other);
 514 }
 515 template<typename Element, typename OtherElement>
 516 inline void vector3_subtract(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 517 {
 518   self.x() -= static_cast<Element>(other.x());
 519   self.y() -= static_cast<Element>(other.y());
 520   self.z() -= static_cast<Element>(other.z());
 521 }
 522 template<typename Element, typename OtherElement>
 523 inline void operator-=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 524 {
 525   vector3_subtract(self, other);
 526 }
 527
 528 template<typename Element, typename OtherElement>
 529 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 530 {
 531   return BasicVector3<Element>(
 532     Element(self.x() * other.x()),
 533     Element(self.y() * other.y()),
 534     Element(self.z() * other.z())
 535   );
 536 }
 537 template<typename Element, typename OtherElement>
 538 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 539 {
 540   return vector3_scaled(self, other);
 541 }
 542 template<typename Element, typename OtherElement>
 543 inline void vector3_scale(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 544 {
 545   self.x() *= static_cast<Element>(other.x());
 546   self.y() *= static_cast<Element>(other.y());
 547   self.z() *= static_cast<Element>(other.z());
 548 }
 549 template<typename Element, typename OtherElement>
 550 inline void operator*=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 551 {
 552   vector3_scale(self, other);
 553 }
 554
 555 template<typename Element, typename OtherElement>
 556 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const OtherElement& scale)
 557 {
 558   return BasicVector3<Element>(
 559     Element(self.x() * scale),
 560     Element(self.y() * scale),
 561     Element(self.z() * scale)
 562   );
 563 }
 564 template<typename Element, typename OtherElement>
 565 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const OtherElement& scale)
 566 {
 567   return vector3_scaled(self, scale);
 568 }
 569 template<typename Element, typename OtherElement>
 570 inline void vector3_scale(BasicVector3<Element>& self, const OtherElement& scale)
 571 {
 572   self.x() *= static_cast<Element>(scale);
 573   self.y() *= static_cast<Element>(scale);
 574   self.z() *= static_cast<Element>(scale);
 575 }
 576 template<typename Element, typename OtherElement>
 577 inline void operator*=(BasicVector3<Element>& self, const OtherElement& scale)
 578 {
 579   vector3_scale(self, scale);
 580 }
 581
 582 template<typename Element, typename OtherElement>
 583 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 584 {
 585   return BasicVector3<Element>(
 586     Element(self.x() / other.x()),
 587     Element(self.y() / other.y()),
 588     Element(self.z() / other.z())
 589   );
 590 }
 591 template<typename Element, typename OtherElement>
 592 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 593 {
 594   return vector3_divided(self, other);
 595 }
 596 template<typename Element, typename OtherElement>
 597 inline void vector3_divide(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 598 {
 599   self.x() /= static_cast<Element>(other.x());
 600   self.y() /= static_cast<Element>(other.y());
 601   self.z() /= static_cast<Element>(other.z());
 602 }
 603 template<typename Element, typename OtherElement>
 604 inline void operator/=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 605 {
 606   vector3_divide(self, other);
 607 }
 608
 609 template<typename Element, typename OtherElement>
 610 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const OtherElement& divisor)
 611 {
 612   return BasicVector3<Element>(
 613     Element(self.x() / divisor),
 614     Element(self.y() / divisor),
 615     Element(self.z() / divisor)
 616   );
 617 }
 618 template<typename Element, typename OtherElement>
 619 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const OtherElement& divisor)
 620 {
 621   return vector3_divided(self, divisor);
 622 }
 623 template<typename Element, typename OtherElement>
 624 inline void vector3_divide(BasicVector3<Element>& self, const OtherElement& divisor)
 625 {
 626   self.x() /= static_cast<Element>(divisor);
 627   self.y() /= static_cast<Element>(divisor);
 628   self.z() /= static_cast<Element>(divisor);
 629 }
 630 template<typename Element, typename OtherElement>
 631 inline void operator/=(BasicVector3<Element>& self, const OtherElement& divisor)
 632 {
 633   vector3_divide(self, divisor);
 634 }
 635
 636 template<typename Element, typename OtherElement>
 637 inline double vector3_dot(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 638 {
 639   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z();
 640 }
 641
 642 template<typename Element>
 643 inline BasicVector3<Element> vector3_mid(const BasicVector3<Element>& begin, const BasicVector3<Element>& end)
 644 {
 645   return vector3_scaled(vector3_added(begin, end), 0.5);
 646 }
 647
 648 template<typename Element, typename OtherElement>
 649 inline BasicVector3<Element> vector3_cross(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 650 {
 651   return BasicVector3<Element>(
 652     Element(self.y() * other.z() - self.z() * other.y()),
 653     Element(self.z() * other.x() - self.x() * other.z()),
 654     Element(self.x() * other.y() - self.y() * other.x())
 655   );
 656 }
 657
 658 template<typename Element>
 659 inline BasicVector3<Element> vector3_negated(const BasicVector3<Element>& self)
 660 {
 661   return BasicVector3<Element>(-self.x(), -self.y(), -self.z());
 662 }
 663 template<typename Element>
 664 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self)
 665 {
 666   return vector3_negated(self);
 667 }
 668
 669 template<typename Element>
 670 inline void vector3_negate(BasicVector3<Element>& self)
 671 {
 672   self = vector3_negated(self);
 673 }
 674
 675 template<typename Element>
 676 inline double vector3_length_squared(const BasicVector3<Element>& self)
 677 {
 678   return vector3_dot(self, self);
 679 }
 680
 681 template<typename Element>
 682 inline double vector3_length(const BasicVector3<Element>& self)
 683 {
 684   return sqrt(vector3_length_squared(self));
 685 }
 686
 687 template<typename Element>
 688 inline Element float_divided(Element f, Element other)
 689 {
 690   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
 691   return f / other;
 692 }
 693
 694 template<typename Element>
 695 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
 696 {
 697   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
 698 }
 699
 700 template<typename Element>
 701 inline void vector3_normalise(BasicVector3<Element>& self)
 702 {
 703   self = vector3_normalised(self);
 704 }
 705
 706
 707 template<typename Element>
 708 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
 709 {
 710   return BasicVector3<Element>(
 711     Element(float_to_integer(self.x())),
 712     Element(float_to_integer(self.y())),
 713     Element(float_to_integer(self.z()))
 714   );
 715 }
 716 template<typename Element>
 717 inline void vector3_snap(BasicVector3<Element>& self)
 718 {
 719   self = vector3_snapped(self);
 720 }
 721 template<typename Element, typename OtherElement>
 722 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
 723 {
 724   return BasicVector3<Element>(
 725     Element(float_snapped(self.x(), snap)),
 726     Element(float_snapped(self.y(), snap)),
 727     Element(float_snapped(self.z(), snap))
 728   );
 729 }
 730 template<typename Element, typename OtherElement>
 731 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
 732 {
 733   self = vector3_snapped(self, snap);
 734 }
 735
 736 inline Vector3 vector3_for_spherical(double theta, double phi)
 737 {
 738   return Vector3(
 739     static_cast<float>(cos(theta) * cos(phi)),
 740     static_cast<float>(sin(theta) * cos(phi)),
 741     static_cast<float>(sin(phi))
 742   );
 743 }
 744
 745
 746 /// \brief A 4-element vector.
 747 template<typename Element>
 748 class BasicVector4
 749 {
 750   Element m_elements[4];
 751 public:
 752
 753   BasicVector4()
 754   {
 755   }
 756   BasicVector4(Element x_, Element y_, Element z_, Element w_)
 757   {
 758     x() = x_;
 759     y() = y_;
 760     z() = z_;
 761     w() = w_;
 762   }
 763   BasicVector4(const BasicVector3<Element>& self, Element w_)
 764   {
 765     x() = self.x();
 766     y() = self.y();
 767     z() = self.z();
 768     w() = w_;
 769   }
 770
 771   Element& x()
 772   {
 773     return m_elements[0];
 774   }
 775   Element x() const
 776   {
 777     return m_elements[0];
 778   }
 779   Element& y()
 780   {
 781     return m_elements[1];
 782   }
 783   Element y() const
 784   {
 785     return m_elements[1];
 786   }
 787   Element& z()
 788   {
 789     return m_elements[2];
 790   }
 791   Element z() const
 792   {
 793     return m_elements[2];
 794   }
 795   Element& w()
 796   {
 797     return m_elements[3];
 798   }
 799   Element w() const
 800   {
 801     return m_elements[3];
 802   }
 803
 804   Element index(std::size_t i) const
 805   {
 806     return m_elements[i];
 807   }
 808   Element& index(std::size_t i)
 809   {
 810     return m_elements[i];
 811   }
 812   Element operator[](std::size_t i) const
 813   {
 814     return m_elements[i];
 815   }
 816   Element& operator[](std::size_t i)
 817   {
 818     return m_elements[i];
 819   }
 820
 821   operator BasicVector3<Element>&()
 822   {
 823     return reinterpret_cast<BasicVector3<Element>&>(*this);
 824   }
 825   operator const BasicVector3<Element>&() const
 826   {
 827     return reinterpret_cast<const BasicVector3<Element>&>(*this);
 828   }
 829
 830   bool operator==(const BasicVector4& other) const
 831   {
 832     return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w();
 833   }
 834 };
 835
 836 /// \brief A 4-element vector stored in single-precision floating-point.
 837 typedef BasicVector4<float> Vector4;
 838
 839
 840 template<typename Element, typename OtherElement>
 841 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 842 {
 843   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
 844 }
 845 template<typename Element, typename OtherElement>
 846 inline bool operator==(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 847 {
 848   return vector4_equal(self, other);
 849 }
 850 template<typename Element, typename OtherElement>
 851 inline bool operator!=(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 852 {
 853   return !vector4_equal(self, other);
 854 }
 855
 856 template<typename Element, typename OtherElement>
 857 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
 858 {
 859   return float_equal_epsilon(self.x(), other.x(), epsilon)
 860     && float_equal_epsilon(self.y(), other.y(), epsilon)
 861     && float_equal_epsilon(self.z(), other.z(), epsilon)
 862     && float_equal_epsilon(self.w(), other.w(), epsilon);
 863 }
 864
 865 template<typename Element>
 866 inline Element* vector4_to_array(BasicVector4<Element>& self)
 867 {
 868   return reinterpret_cast<Element*>(&self);
 869 }
 870 template<typename Element>
 871 inline const float* vector4_to_array(const BasicVector4<Element>& self)
 872 {
 873   return reinterpret_cast<const Element*>(&self);
 874 }
 875
 876 template<typename Element>
 877 inline Vector3& vector4_to_vector3(BasicVector4<Element>& self)
 878 {
 879   return reinterpret_cast<BasicVector3<Element>&>(self);
 880 }
 881 template<typename Element>
 882 inline const Vector3& vector4_to_vector3(const BasicVector4<Element>& self)
 883 {
 884   return reinterpret_cast<const BasicVector3<Element>&>(self);
 885 }
 886
 887 template<typename Element, typename OtherElement>
 888 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 889 {
 890   return BasicVector4<Element>(
 891     float(self.x() + other.x()),
 892     float(self.y() + other.y()),
 893     float(self.z() + other.z()),
 894     float(self.w() + other.w())
 895   );
 896 }
 897 template<typename Element, typename OtherElement>
 898 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 899 {
 900   return vector4_added(self, other);
 901 }
 902 template<typename Element, typename OtherElement>
 903 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 904 {
 905   self.x() += static_cast<float>(other.x());
 906   self.y() += static_cast<float>(other.y());
 907   self.z() += static_cast<float>(other.z());
 908   self.w() += static_cast<float>(other.w());
 909 }
 910 template<typename Element, typename OtherElement>
 911 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 912 {
 913   vector4_add(self, other);
 914 }
 915
 916 template<typename Element, typename OtherElement>
 917 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 918 {
 919   return BasicVector4<Element>(
 920     float(self.x() - other.x()),
 921     float(self.y() - other.y()),
 922     float(self.z() - other.z()),
 923     float(self.w() - other.w())
 924   );
 925 }
 926 template<typename Element, typename OtherElement>
 927 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 928 {
 929   return vector4_subtracted(self, other);
 930 }
 931 template<typename Element, typename OtherElement>
 932 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 933 {
 934   self.x() -= static_cast<float>(other.x());
 935   self.y() -= static_cast<float>(other.y());
 936   self.z() -= static_cast<float>(other.z());
 937   self.w() -= static_cast<float>(other.w());
 938 }
 939 template<typename Element, typename OtherElement>
 940 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 941 {
 942   vector4_subtract(self, other);
 943 }
 944
 945 template<typename Element, typename OtherElement>
 946 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 947 {
 948   return BasicVector4<Element>(
 949     float(self.x() * other.x()),
 950     float(self.y() * other.y()),
 951     float(self.z() * other.z()),
 952     float(self.w() * other.w())
 953   );
 954 }
 955 template<typename Element, typename OtherElement>
 956 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 957 {
 958   return vector4_scaled(self, other);
 959 }
 960 template<typename Element, typename OtherElement>
 961 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 962 {
 963   self.x() *= static_cast<float>(other.x());
 964   self.y() *= static_cast<float>(other.y());
 965   self.z() *= static_cast<float>(other.z());
 966   self.w() *= static_cast<float>(other.w());
 967 }
 968 template<typename Element, typename OtherElement>
 969 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 970 {
 971   vector4_scale(self, other);
 972 }
 973
 974 template<typename Element, typename OtherElement>
 975 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
 976 {
 977   return BasicVector4<Element>(
 978     float(self.x() * scale),
 979     float(self.y() * scale),
 980     float(self.z() * scale),
 981     float(self.w() * scale)
 982   );
 983 }
 984 template<typename Element, typename OtherElement>
 985 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
 986 {
 987   return vector4_scaled(self, scale);
 988 }
 989 template<typename Element, typename OtherElement>
 990 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
 991 {
 992   self.x() *= static_cast<float>(scale);
 993   self.y() *= static_cast<float>(scale);
 994   self.z() *= static_cast<float>(scale);
 995   self.w() *= static_cast<float>(scale);
 996 }
 997 template<typename Element, typename OtherElement>
 998 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
 999 {
1000   vector4_scale(self, scale);
1001 }
1002
1003 template<typename Element, typename OtherElement>
1004 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
1005 {
1006   return BasicVector4<Element>(
1007     float(self.x() / divisor),
1008     float(self.y() / divisor),
1009     float(self.z() / divisor),
1010     float(self.w() / divisor)
1011   );
1012 }
1013 template<typename Element, typename OtherElement>
1014 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
1015 {
1016   return vector4_divided(self, divisor);
1017 }
1018 template<typename Element, typename OtherElement>
1019 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
1020 {
1021   self.x() /= divisor;
1022   self.y() /= divisor;
1023   self.z() /= divisor;
1024   self.w() /= divisor;
1025 }
1026 template<typename Element, typename OtherElement>
1027 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
1028 {
1029   vector4_divide(self, divisor);
1030 }
1031
1032 template<typename Element, typename OtherElement>
1033 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
1034 {
1035   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
1036 }
1037
1038 template<typename Element>
1039 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
1040 {
1041   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
1042 }
1043
1044 #endif