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 #include "generic/vector.h"
  29
  30 #if defined (_MSC_VER)
  31
  32 inline int lrint (double flt)
  33 {
  34   int i;
  35
  36         _asm
  37         {
  38     fld flt
  39                 fistp i
  40   };
  41
  42         return i;
  43 }
  44
  45 inline __int64 llrint(double f)
  46 {
  47   return static_cast<__int64>(f + 0.5);
  48 }
  49
  50 #elif defined(__FreeBSD__)
  51
  52 inline long lrint(double f)
  53 {
  54   return static_cast<long>(f + 0.5);
  55 }
  56
  57 inline long long llrint(double f)
  58 {
  59   return static_cast<long long>(f + 0.5);
  60 }
  61
  62 #elif defined(__GNUC__)
  63
  64  // lrint is part of ISO C99
  65 #define _ISOC9X_SOURCE  1
  66 #define _ISOC99_SOURCE  1
  67
  68 #define __USE_ISOC9X    1
  69 #define __USE_ISOC99    1
  70
  71 #else
  72 #error "unsupported platform"
  73 #endif
  74
  75 #include <cmath>
  76 #include <float.h>
  77 #include <algorithm>
  78
  79
  80 //#include "debugging/debugging.h"
  81
  82 /// \brief Returns true if \p self is equal to other \p other within \p epsilon.
  83 template<typename Element, typename OtherElement>
  84 inline bool float_equal_epsilon(const Element& self, const OtherElement& other, const Element& epsilon)
  85 {
  86   return fabs(other - self) < epsilon;
  87 }
  88
  89 /// \brief Returns the value midway between \p self and \p other.
  90 template<typename Element>
  91 inline Element float_mid(const Element& self, const Element& other)
  92 {
  93   return Element((self + other) * 0.5);
  94 }
  95
  96 /// \brief Returns \p f rounded to the nearest integer. Note that this is not the same behaviour as casting from float to int.
  97 template<typename Element>
  98 inline int float_to_integer(const Element& f)
  99 {
 100   return lrint(f);
 101 }
 102
 103 /// \brief Returns \p f rounded to the nearest multiple of \p snap.
 104 template<typename Element, typename OtherElement>
 105 inline Element float_snapped(const Element& f, const OtherElement& snap)
 106 {
 107   //return Element(float_to_integer(f / snap) * snap);
 108   return Element(llrint(f / snap) * snap); // llrint has more significant bits
 109 }
 110
 111 /// \brief Returns true if \p f has no decimal fraction part.
 112 template<typename Element>
 113 inline bool float_is_integer(const Element& f)
 114 {
 115   return f == Element(float_to_integer(f));
 116 }
 117
 118 /// \brief Returns \p self modulated by the range [0, \p modulus)
 119 /// \p self must be in the range [\p -modulus, \p modulus)
 120 template<typename Element, typename ModulusElement>
 121 inline Element float_mod_range(const Element& self, const ModulusElement& modulus)
 122 {
 123   return Element((self < 0.0) ? self + modulus : self);
 124 }
 125
 126 /// \brief Returns \p self modulated by the range [0, \p modulus)
 127 template<typename Element, typename ModulusElement>
 128 inline Element float_mod(const Element& self, const ModulusElement& modulus)
 129 {
 130   return float_mod_range(Element(fmod(static_cast<double>(self), static_cast<double>(modulus))), modulus);
 131 }
 132
 133
 134 template<typename Element, typename OtherElement>
 135 inline BasicVector2<Element> vector2_added(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 136 {
 137   return BasicVector2<Element>(
 138     Element(self.x() + other.x()),
 139     Element(self.y() + other.y())
 140   );
 141 }
 142 template<typename Element, typename OtherElement>
 143 inline BasicVector2<Element> operator+(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 144 {
 145   return vector2_added(self, other);
 146 }
 147 template<typename Element, typename OtherElement>
 148 inline void vector2_add(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 149 {
 150   self.x() += Element(other.x());
 151   self.y() += Element(other.y());
 152 }
 153 template<typename Element, typename OtherElement>
 154 inline void operator+=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 155 {
 156   vector2_add(self, other);
 157 }
 158
 159
 160 template<typename Element, typename OtherElement>
 161 inline BasicVector2<Element> vector2_subtracted(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_subtracted(self, other);
 172 }
 173 template<typename Element, typename OtherElement>
 174 inline void vector2_subtract(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 175 {
 176   self.x() -= Element(other.x());
 177   self.y() -= lement(other.y());
 178 }
 179 template<typename Element, typename OtherElement>
 180 inline void operator-=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 181 {
 182   vector2_subtract(self, other);
 183 }
 184
 185
 186 template<typename Element, typename OtherElement>
 187 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, OtherElement other)
 188 {
 189   return BasicVector2<Element>(
 190     Element(self.x() * other),
 191     Element(self.y() * other)
 192   );
 193 }
 194 template<typename Element, typename OtherElement>
 195 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, OtherElement other)
 196 {
 197   return vector2_scaled(self, other);
 198 }
 199 template<typename Element, typename OtherElement>
 200 inline void vector2_scale(BasicVector2<Element>& self, OtherElement other)
 201 {
 202   self.x() *= Element(other);
 203   self.y() *= Element(other);
 204 }
 205 template<typename Element, typename OtherElement>
 206 inline void operator*=(BasicVector2<Element>& self, OtherElement other)
 207 {
 208   vector2_scale(self, other);
 209 }
 210
 211
 212 template<typename Element, typename OtherElement>
 213 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 214 {
 215   return BasicVector2<Element>(
 216     Element(self.x() * other.x()),
 217     Element(self.y() * other.y())
 218   );
 219 }
 220 template<typename Element, typename OtherElement>
 221 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 222 {
 223   return vector2_scaled(self, other);
 224 }
 225 template<typename Element, typename OtherElement>
 226 inline void vector2_scale(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 227 {
 228   self.x() *= Element(other.x());
 229   self.y() *= Element(other.y());
 230 }
 231 template<typename Element, typename OtherElement>
 232 inline void operator*=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 233 {
 234   vector2_scale(self, other);
 235 }
 236
 237 template<typename Element, typename OtherElement>
 238 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 239 {
 240   return BasicVector2<Element>(
 241     Element(self.x() / other.x()),
 242     Element(self.y() / other.y())
 243   );
 244 }
 245 template<typename Element, typename OtherElement>
 246 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 247 {
 248   return vector2_divided(self, other);
 249 }
 250 template<typename Element, typename OtherElement>
 251 inline void vector2_divide(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 252 {
 253   self.x() /= Element(other.x());
 254   self.y() /= Element(other.y());
 255 }
 256 template<typename Element, typename OtherElement>
 257 inline void operator/=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 258 {
 259   vector2_divide(self, other);
 260 }
 261
 262
 263 template<typename Element, typename OtherElement>
 264 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, OtherElement other)
 265 {
 266   return BasicVector2<Element>(
 267     Element(self.x() / other),
 268     Element(self.y() / other)
 269   );
 270 }
 271 template<typename Element, typename OtherElement>
 272 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, OtherElement other)
 273 {
 274   return vector2_divided(self, other);
 275 }
 276 template<typename Element, typename OtherElement>
 277 inline void vector2_divide(BasicVector2<Element>& self, OtherElement other)
 278 {
 279   self.x() /= Element(other);
 280   self.y() /= Element(other);
 281 }
 282 template<typename Element, typename OtherElement>
 283 inline void operator/=(BasicVector2<Element>& self, OtherElement other)
 284 {
 285   vector2_divide(self, other);
 286 }
 287
 288 template<typename Element, typename OtherElement>
 289 inline double vector2_dot(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 290 {
 291   return self.x() * other.x() + self.y() * other.y();
 292 }
 293
 294 template<typename Element>
 295 inline double vector2_length_squared(const BasicVector2<Element>& self)
 296 {
 297   return vector2_dot(self, self);
 298 }
 299
 300 template<typename Element>
 301 inline double vector2_length(const BasicVector2<Element>& self)
 302 {
 303   return sqrt(vector2_length_squared(self));
 304 }
 305
 306 template<typename Element, typename OtherElement>
 307 inline double vector2_cross(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 308 {
 309   return self.x() * other.y() - self.y() * other.x();
 310 }
 311
 312 const Vector3 g_vector3_identity(0, 0, 0);
 313 const Vector3 g_vector3_max = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
 314 const Vector3 g_vector3_axis_x(1, 0, 0);
 315 const Vector3 g_vector3_axis_y(0, 1, 0);
 316 const Vector3 g_vector3_axis_z(0, 0, 1);
 317
 318 const Vector3 g_vector3_axes[3] = { g_vector3_axis_x, g_vector3_axis_y, g_vector3_axis_z };
 319
 320 template<typename Element, typename OtherElement>
 321 inline void vector3_swap(BasicVector3<Element>& self, BasicVector3<OtherElement>& other)
 322 {
 323   std::swap(self.x(), other.x());
 324   std::swap(self.y(), other.y());
 325   std::swap(self.z(), other.z());
 326 }
 327
 328 template<typename Element, typename OtherElement>
 329 inline bool vector3_equal(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 330 {
 331   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z();
 332 }
 333 template<typename Element, typename OtherElement>
 334 inline bool operator==(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 335 {
 336   return vector3_equal(self, other);
 337 }
 338 template<typename Element, typename OtherElement>
 339 inline bool operator!=(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 340 {
 341   return !vector3_equal(self, other);
 342 }
 343
 344
 345 template<typename Element, typename OtherElement, typename Epsilon>
 346 inline bool vector3_equal_epsilon(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other, Epsilon epsilon)
 347 {
 348   return float_equal_epsilon(self.x(), other.x(), epsilon)
 349     && float_equal_epsilon(self.y(), other.y(), epsilon)
 350     && float_equal_epsilon(self.z(), other.z(), epsilon);
 351 }
 352
 353
 354
 355 template<typename Element, typename OtherElement>
 356 inline BasicVector3<Element> vector3_added(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 357 {
 358   return BasicVector3<Element>(
 359     Element(self.x() + other.x()),
 360     Element(self.y() + other.y()),
 361     Element(self.z() + other.z())
 362   );
 363 }
 364 template<typename Element, typename OtherElement>
 365 inline BasicVector3<Element> operator+(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 366 {
 367   return vector3_added(self, other);
 368 }
 369 template<typename Element, typename OtherElement>
 370 inline void vector3_add(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 371 {
 372   self.x() += static_cast<Element>(other.x());
 373   self.y() += static_cast<Element>(other.y());
 374   self.z() += static_cast<Element>(other.z());
 375 }
 376 template<typename Element, typename OtherElement>
 377 inline void operator+=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 378 {
 379   vector3_add(self, other);
 380 }
 381
 382 template<typename Element, typename OtherElement>
 383 inline BasicVector3<Element> vector3_subtracted(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 384 {
 385   return BasicVector3<Element>(
 386     Element(self.x() - other.x()),
 387     Element(self.y() - other.y()),
 388     Element(self.z() - other.z())
 389   );
 390 }
 391 template<typename Element, typename OtherElement>
 392 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 393 {
 394   return vector3_subtracted(self, other);
 395 }
 396 template<typename Element, typename OtherElement>
 397 inline void vector3_subtract(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 398 {
 399   self.x() -= static_cast<Element>(other.x());
 400   self.y() -= static_cast<Element>(other.y());
 401   self.z() -= static_cast<Element>(other.z());
 402 }
 403 template<typename Element, typename OtherElement>
 404 inline void operator-=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 405 {
 406   vector3_subtract(self, other);
 407 }
 408
 409 template<typename Element, typename OtherElement>
 410 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 411 {
 412   return BasicVector3<Element>(
 413     Element(self.x() * other.x()),
 414     Element(self.y() * other.y()),
 415     Element(self.z() * other.z())
 416   );
 417 }
 418 template<typename Element, typename OtherElement>
 419 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 420 {
 421   return vector3_scaled(self, other);
 422 }
 423 template<typename Element, typename OtherElement>
 424 inline void vector3_scale(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 425 {
 426   self.x() *= static_cast<Element>(other.x());
 427   self.y() *= static_cast<Element>(other.y());
 428   self.z() *= static_cast<Element>(other.z());
 429 }
 430 template<typename Element, typename OtherElement>
 431 inline void operator*=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 432 {
 433   vector3_scale(self, other);
 434 }
 435
 436 template<typename Element, typename OtherElement>
 437 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const OtherElement& scale)
 438 {
 439   return BasicVector3<Element>(
 440     Element(self.x() * scale),
 441     Element(self.y() * scale),
 442     Element(self.z() * scale)
 443   );
 444 }
 445 template<typename Element, typename OtherElement>
 446 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const OtherElement& scale)
 447 {
 448   return vector3_scaled(self, scale);
 449 }
 450 template<typename Element, typename OtherElement>
 451 inline void vector3_scale(BasicVector3<Element>& self, const OtherElement& scale)
 452 {
 453   self.x() *= static_cast<Element>(scale);
 454   self.y() *= static_cast<Element>(scale);
 455   self.z() *= static_cast<Element>(scale);
 456 }
 457 template<typename Element, typename OtherElement>
 458 inline void operator*=(BasicVector3<Element>& self, const OtherElement& scale)
 459 {
 460   vector3_scale(self, scale);
 461 }
 462
 463 template<typename Element, typename OtherElement>
 464 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 465 {
 466   return BasicVector3<Element>(
 467     Element(self.x() / other.x()),
 468     Element(self.y() / other.y()),
 469     Element(self.z() / other.z())
 470   );
 471 }
 472 template<typename Element, typename OtherElement>
 473 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 474 {
 475   return vector3_divided(self, other);
 476 }
 477 template<typename Element, typename OtherElement>
 478 inline void vector3_divide(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 479 {
 480   self.x() /= static_cast<Element>(other.x());
 481   self.y() /= static_cast<Element>(other.y());
 482   self.z() /= static_cast<Element>(other.z());
 483 }
 484 template<typename Element, typename OtherElement>
 485 inline void operator/=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 486 {
 487   vector3_divide(self, other);
 488 }
 489
 490 template<typename Element, typename OtherElement>
 491 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const OtherElement& divisor)
 492 {
 493   return BasicVector3<Element>(
 494     Element(self.x() / divisor),
 495     Element(self.y() / divisor),
 496     Element(self.z() / divisor)
 497   );
 498 }
 499 template<typename Element, typename OtherElement>
 500 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const OtherElement& divisor)
 501 {
 502   return vector3_divided(self, divisor);
 503 }
 504 template<typename Element, typename OtherElement>
 505 inline void vector3_divide(BasicVector3<Element>& self, const OtherElement& divisor)
 506 {
 507   self.x() /= static_cast<Element>(divisor);
 508   self.y() /= static_cast<Element>(divisor);
 509   self.z() /= static_cast<Element>(divisor);
 510 }
 511 template<typename Element, typename OtherElement>
 512 inline void operator/=(BasicVector3<Element>& self, const OtherElement& divisor)
 513 {
 514   vector3_divide(self, divisor);
 515 }
 516
 517 template<typename Element, typename OtherElement>
 518 inline double vector3_dot(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 519 {
 520   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z();
 521 }
 522
 523 template<typename Element>
 524 inline BasicVector3<Element> vector3_mid(const BasicVector3<Element>& begin, const BasicVector3<Element>& end)
 525 {
 526   return vector3_scaled(vector3_added(begin, end), 0.5);
 527 }
 528
 529 template<typename Element, typename OtherElement>
 530 inline BasicVector3<Element> vector3_cross(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 531 {
 532   return BasicVector3<Element>(
 533     Element(self.y() * other.z() - self.z() * other.y()),
 534     Element(self.z() * other.x() - self.x() * other.z()),
 535     Element(self.x() * other.y() - self.y() * other.x())
 536   );
 537 }
 538
 539 template<typename Element>
 540 inline BasicVector3<Element> vector3_negated(const BasicVector3<Element>& self)
 541 {
 542   return BasicVector3<Element>(-self.x(), -self.y(), -self.z());
 543 }
 544 template<typename Element>
 545 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self)
 546 {
 547   return vector3_negated(self);
 548 }
 549
 550 template<typename Element>
 551 inline void vector3_negate(BasicVector3<Element>& self)
 552 {
 553   self = vector3_negated(self);
 554 }
 555
 556 template<typename Element>
 557 inline double vector3_length_squared(const BasicVector3<Element>& self)
 558 {
 559   return vector3_dot(self, self);
 560 }
 561
 562 template<typename Element>
 563 inline double vector3_length(const BasicVector3<Element>& self)
 564 {
 565   return sqrt(vector3_length_squared(self));
 566 }
 567
 568 template<typename Element>
 569 inline Element float_divided(Element f, Element other)
 570 {
 571   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
 572   return f / other;
 573 }
 574
 575 template<typename Element>
 576 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
 577 {
 578   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
 579 }
 580
 581 template<typename Element>
 582 inline void vector3_normalise(BasicVector3<Element>& self)
 583 {
 584   self = vector3_normalised(self);
 585 }
 586
 587
 588 template<typename Element>
 589 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
 590 {
 591   return BasicVector3<Element>(
 592     Element(float_to_integer(self.x())),
 593     Element(float_to_integer(self.y())),
 594     Element(float_to_integer(self.z()))
 595   );
 596 }
 597 template<typename Element>
 598 inline void vector3_snap(BasicVector3<Element>& self)
 599 {
 600   self = vector3_snapped(self);
 601 }
 602 template<typename Element, typename OtherElement>
 603 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
 604 {
 605   return BasicVector3<Element>(
 606     Element(float_snapped(self.x(), snap)),
 607     Element(float_snapped(self.y(), snap)),
 608     Element(float_snapped(self.z(), snap))
 609   );
 610 }
 611 template<typename Element, typename OtherElement>
 612 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
 613 {
 614   self = vector3_snapped(self, snap);
 615 }
 616
 617 inline Vector3 vector3_for_spherical(double theta, double phi)
 618 {
 619   return Vector3(
 620     static_cast<float>(cos(theta) * cos(phi)),
 621     static_cast<float>(sin(theta) * cos(phi)),
 622     static_cast<float>(sin(phi))
 623   );
 624 }
 625
 626
 627
 628
 629 template<typename Element, typename OtherElement>
 630 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 631 {
 632   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
 633 }
 634 template<typename Element, typename OtherElement>
 635 inline bool operator==(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 636 {
 637   return vector4_equal(self, other);
 638 }
 639 template<typename Element, typename OtherElement>
 640 inline bool operator!=(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 641 {
 642   return !vector4_equal(self, other);
 643 }
 644
 645 template<typename Element, typename OtherElement>
 646 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
 647 {
 648   return float_equal_epsilon(self.x(), other.x(), epsilon)
 649     && float_equal_epsilon(self.y(), other.y(), epsilon)
 650     && float_equal_epsilon(self.z(), other.z(), epsilon)
 651     && float_equal_epsilon(self.w(), other.w(), epsilon);
 652 }
 653
 654 template<typename Element, typename OtherElement>
 655 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 656 {
 657   return BasicVector4<Element>(
 658     float(self.x() + other.x()),
 659     float(self.y() + other.y()),
 660     float(self.z() + other.z()),
 661     float(self.w() + other.w())
 662   );
 663 }
 664 template<typename Element, typename OtherElement>
 665 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 666 {
 667   return vector4_added(self, other);
 668 }
 669 template<typename Element, typename OtherElement>
 670 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 671 {
 672   self.x() += static_cast<float>(other.x());
 673   self.y() += static_cast<float>(other.y());
 674   self.z() += static_cast<float>(other.z());
 675   self.w() += static_cast<float>(other.w());
 676 }
 677 template<typename Element, typename OtherElement>
 678 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 679 {
 680   vector4_add(self, other);
 681 }
 682
 683 template<typename Element, typename OtherElement>
 684 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 685 {
 686   return BasicVector4<Element>(
 687     float(self.x() - other.x()),
 688     float(self.y() - other.y()),
 689     float(self.z() - other.z()),
 690     float(self.w() - other.w())
 691   );
 692 }
 693 template<typename Element, typename OtherElement>
 694 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 695 {
 696   return vector4_subtracted(self, other);
 697 }
 698 template<typename Element, typename OtherElement>
 699 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 700 {
 701   self.x() -= static_cast<float>(other.x());
 702   self.y() -= static_cast<float>(other.y());
 703   self.z() -= static_cast<float>(other.z());
 704   self.w() -= static_cast<float>(other.w());
 705 }
 706 template<typename Element, typename OtherElement>
 707 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 708 {
 709   vector4_subtract(self, other);
 710 }
 711
 712 template<typename Element, typename OtherElement>
 713 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 714 {
 715   return BasicVector4<Element>(
 716     float(self.x() * other.x()),
 717     float(self.y() * other.y()),
 718     float(self.z() * other.z()),
 719     float(self.w() * other.w())
 720   );
 721 }
 722 template<typename Element, typename OtherElement>
 723 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 724 {
 725   return vector4_scaled(self, other);
 726 }
 727 template<typename Element, typename OtherElement>
 728 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 729 {
 730   self.x() *= static_cast<float>(other.x());
 731   self.y() *= static_cast<float>(other.y());
 732   self.z() *= static_cast<float>(other.z());
 733   self.w() *= static_cast<float>(other.w());
 734 }
 735 template<typename Element, typename OtherElement>
 736 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 737 {
 738   vector4_scale(self, other);
 739 }
 740
 741 template<typename Element, typename OtherElement>
 742 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
 743 {
 744   return BasicVector4<Element>(
 745     float(self.x() * scale),
 746     float(self.y() * scale),
 747     float(self.z() * scale),
 748     float(self.w() * scale)
 749   );
 750 }
 751 template<typename Element, typename OtherElement>
 752 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
 753 {
 754   return vector4_scaled(self, scale);
 755 }
 756 template<typename Element, typename OtherElement>
 757 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
 758 {
 759   self.x() *= static_cast<float>(scale);
 760   self.y() *= static_cast<float>(scale);
 761   self.z() *= static_cast<float>(scale);
 762   self.w() *= static_cast<float>(scale);
 763 }
 764 template<typename Element, typename OtherElement>
 765 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
 766 {
 767   vector4_scale(self, scale);
 768 }
 769
 770 template<typename Element, typename OtherElement>
 771 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
 772 {
 773   return BasicVector4<Element>(
 774     float(self.x() / divisor),
 775     float(self.y() / divisor),
 776     float(self.z() / divisor),
 777     float(self.w() / divisor)
 778   );
 779 }
 780 template<typename Element, typename OtherElement>
 781 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
 782 {
 783   return vector4_divided(self, divisor);
 784 }
 785 template<typename Element, typename OtherElement>
 786 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
 787 {
 788   self.x() /= divisor;
 789   self.y() /= divisor;
 790   self.z() /= divisor;
 791   self.w() /= divisor;
 792 }
 793 template<typename Element, typename OtherElement>
 794 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
 795 {
 796   vector4_divide(self, divisor);
 797 }
 798
 799 template<typename Element, typename OtherElement>
 800 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 801 {
 802   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
 803 }
 804
 805 template<typename Element>
 806 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
 807 {
 808   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
 809 }
 810
 811 #endif