source/maths/FixedVector3D.h

   1 /* Copyright (C) 2020 Wildfire Games.
   2  * This file is part of 0 A.D.
   3  *
   4  * 0 A.D. is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * 0 A.D. is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #ifndef INCLUDED_FIXED_VECTOR3D
  19 #define INCLUDED_FIXED_VECTOR3D
  20
  21 #include "maths/Fixed.h"
  22 #include "maths/Sqrt.h"
  23
  24 class CFixedVector3D
  25 {
  26 public:
  27         fixed X, Y, Z;
  28
  29         CFixedVector3D() { }
  30
  31         CFixedVector3D(fixed X, fixed Y, fixed Z) : X(X), Y(Y), Z(Z) { }
  32
  33         /// Vector equality
  34         bool operator==(const CFixedVector3D& v) const
  35         {
  36                 return (X == v.X && Y == v.Y && Z == v.Z);
  37         }
  38
  39         /// Vector inequality
  40         bool operator!=(const CFixedVector3D& v) const
  41         {
  42                 return (X != v.X || Y != v.Y || Z != v.Z);
  43         }
  44
  45         /// Vector addition
  46         CFixedVector3D operator+(const CFixedVector3D& v) const
  47         {
  48                 return CFixedVector3D(X + v.X, Y + v.Y, Z + v.Z);
  49         }
  50
  51         /// Vector subtraction
  52         CFixedVector3D operator-(const CFixedVector3D& v) const
  53         {
  54                 return CFixedVector3D(X - v.X, Y - v.Y, Z - v.Z);
  55         }
  56
  57         /// Negation
  58         CFixedVector3D operator-() const
  59         {
  60                 return CFixedVector3D(-X, -Y, -Z);
  61         }
  62
  63         /// Vector addition
  64         CFixedVector3D& operator+=(const CFixedVector3D& v)
  65         {
  66                 *this = *this + v;
  67                 return *this;
  68         }
  69
  70         /// Vector subtraction
  71         CFixedVector3D& operator-=(const CFixedVector3D& v)
  72         {
  73                 *this = *this - v;
  74                 return *this;
  75         }
  76
  77
  78         /**
  79          * Returns the length of the vector.
  80          * Will not overflow if the result can be represented as type 'fixed'.
  81          */
  82         fixed Length() const
  83         {
  84                 // Do intermediate calculations with 64-bit ints to avoid overflows
  85                 u64 xx = SQUARE_U64_FIXED(X);
  86                 u64 yy = SQUARE_U64_FIXED(Y);
  87                 u64 zz = SQUARE_U64_FIXED(Z);
  88                 u64 t = xx + yy;
  89                 CheckUnsignedAdditionOverflow(t, xx, L"Overflow in CFixedVector3D::Length() part 1")
  90
  91                 u64 d2 = t + zz;
  92                 CheckUnsignedAdditionOverflow(d2, t, L"Overflow in CFixedVector3D::Length() part 2")
  93
  94                 u32 d = isqrt64(d2);
  95
  96                 CheckU32CastOverflow(d, i32, L"Overflow in CFixedVector3D::Length() part 3")
  97                 fixed r;
  98                 r.SetInternalValue((i32)d);
  99                 return r;
 100         }
 101
 102         /**
 103          * Normalize the vector so that length is close to 1.
 104          * If length is 0, does nothing.
 105          */
 106         void Normalize()
 107         {
 108                 fixed l = Length();
 109                 if (!l.IsZero())
 110                 {
 111                         X = X / l;
 112                         Y = Y / l;
 113                         Z = Z / l;
 114                 }
 115         }
 116
 117         /**
 118          * Normalize the vector so that length is close to n.
 119          * If length is 0, does nothing.
 120          */
 121         void Normalize(fixed n)
 122         {
 123                 fixed l = Length();
 124                 if (!l.IsZero())
 125                 {
 126                         X = X.MulDiv(n, l);
 127                         Y = Y.MulDiv(n, l);
 128                         Z = Z.MulDiv(n, l);
 129                 }
 130         }
 131
 132         /**
 133          * Compute the cross product of this vector with another.
 134          */
 135         CFixedVector3D Cross(const CFixedVector3D& v)
 136         {
 137                 i64 y_vz = MUL_I64_I32_I32(Y.GetInternalValue(), v.Z.GetInternalValue());
 138                 i64 z_vy = MUL_I64_I32_I32(Z.GetInternalValue(), v.Y.GetInternalValue());
 139                 CheckSignedSubtractionOverflow(i64, y_vz, z_vy, L"Overflow in CFixedVector3D::Cross() part 1", L"Underflow in CFixedVector3D::Cross() part 1")
 140                 i64 x = y_vz - z_vy;
 141                 x >>= fixed::fract_bits;
 142
 143                 i64 z_vx = MUL_I64_I32_I32(Z.GetInternalValue(), v.X.GetInternalValue());
 144                 i64 x_vz = MUL_I64_I32_I32(X.GetInternalValue(), v.Z.GetInternalValue());
 145                 CheckSignedSubtractionOverflow(i64, z_vx, x_vz, L"Overflow in CFixedVector3D::Cross() part 2", L"Underflow in CFixedVector3D::Cross() part 2")
 146                 i64 y = z_vx - x_vz;
 147                 y >>= fixed::fract_bits;
 148
 149                 i64 x_vy = MUL_I64_I32_I32(X.GetInternalValue(), v.Y.GetInternalValue());
 150                 i64 y_vx = MUL_I64_I32_I32(Y.GetInternalValue(), v.X.GetInternalValue());
 151                 CheckSignedSubtractionOverflow(i64, x_vy, y_vx, L"Overflow in CFixedVector3D::Cross() part 3", L"Underflow in CFixedVector3D::Cross() part 3")
 152                 i64 z = x_vy - y_vx;
 153                 z >>= fixed::fract_bits;
 154
 155                 CheckCastOverflow(x, i32, L"Overflow in CFixedVector3D::Cross() part 4", L"Underflow in CFixedVector3D::Cross() part 4")
 156                 CheckCastOverflow(y, i32, L"Overflow in CFixedVector3D::Cross() part 5", L"Underflow in CFixedVector3D::Cross() part 5")
 157                 CheckCastOverflow(z, i32, L"Overflow in CFixedVector3D::Cross() part 6", L"Underflow in CFixedVector3D::Cross() part 6")
 158                 CFixedVector3D ret;
 159                 ret.X.SetInternalValue((i32)x);
 160                 ret.Y.SetInternalValue((i32)y);
 161                 ret.Z.SetInternalValue((i32)z);
 162                 return ret;
 163         }
 164
 165         /**
 166          * Compute the dot product of this vector with another.
 167          */
 168         fixed Dot(const CFixedVector3D& v)
 169         {
 170                 i64 x = MUL_I64_I32_I32(X.GetInternalValue(), v.X.GetInternalValue());
 171                 i64 y = MUL_I64_I32_I32(Y.GetInternalValue(), v.Y.GetInternalValue());
 172                 i64 z = MUL_I64_I32_I32(Z.GetInternalValue(), v.Z.GetInternalValue());
 173                 CheckSignedAdditionOverflow(i64, x, y, L"Overflow in CFixedVector3D::Dot() part 1", L"Underflow in CFixedVector3D::Dot() part 1")
 174                 i64 t = x + y;
 175
 176                 CheckSignedAdditionOverflow(i64, t, z, L"Overflow in CFixedVector3D::Dot() part 2", L"Underflow in CFixedVector3D::Dot() part 2")
 177                 i64 sum = t + z;
 178                 sum >>= fixed::fract_bits;
 179                 CheckCastOverflow(sum, i32, L"Overflow in CFixedVector3D::Dot() part 3", L"Underflow in CFixedVector3D::Dot() part 3")
 180
 181                 fixed ret;
 182                 ret.SetInternalValue((i32)sum);
 183                 return ret;
 184         }
 185 };
 186
 187 #endif // INCLUDED_FIXED_VECTOR3D