Line data Source code
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 73 : CFixedVector3D() { }
30 :
31 93 : CFixedVector3D(fixed X, fixed Y, fixed Z) : X(X), Y(Y), Z(Z) { }
32 :
33 : /// Vector equality
34 12 : bool operator==(const CFixedVector3D& v) const
35 : {
36 12 : 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 15 : CFixedVector3D operator+(const CFixedVector3D& v) const
47 : {
48 15 : return CFixedVector3D(X + v.X, Y + v.Y, Z + v.Z);
49 : }
50 :
51 : /// Vector subtraction
52 22 : CFixedVector3D operator-(const CFixedVector3D& v) const
53 : {
54 22 : return CFixedVector3D(X - v.X, Y - v.Y, Z - v.Z);
55 : }
56 :
57 : /// Negation
58 1 : CFixedVector3D operator-() const
59 : {
60 1 : return CFixedVector3D(-X, -Y, -Z);
61 : }
62 :
63 : /// Vector addition
64 1 : CFixedVector3D& operator+=(const CFixedVector3D& v)
65 : {
66 1 : *this = *this + v;
67 1 : return *this;
68 : }
69 :
70 : /// Vector subtraction
71 13 : CFixedVector3D& operator-=(const CFixedVector3D& v)
72 : {
73 13 : *this = *this - v;
74 13 : 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 29 : fixed Length() const
83 : {
84 : // Do intermediate calculations with 64-bit ints to avoid overflows
85 29 : u64 xx = SQUARE_U64_FIXED(X);
86 29 : u64 yy = SQUARE_U64_FIXED(Y);
87 29 : u64 zz = SQUARE_U64_FIXED(Z);
88 29 : u64 t = xx + yy;
89 29 : CheckUnsignedAdditionOverflow(t, xx, L"Overflow in CFixedVector3D::Length() part 1")
90 :
91 29 : u64 d2 = t + zz;
92 29 : CheckUnsignedAdditionOverflow(d2, t, L"Overflow in CFixedVector3D::Length() part 2")
93 :
94 29 : u32 d = isqrt64(d2);
95 :
96 29 : CheckU32CastOverflow(d, i32, L"Overflow in CFixedVector3D::Length() part 3")
97 29 : fixed r;
98 29 : r.SetInternalValue((i32)d);
99 29 : 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 19 : void Normalize()
107 : {
108 19 : fixed l = Length();
109 19 : if (!l.IsZero())
110 : {
111 18 : X = X / l;
112 18 : Y = Y / l;
113 18 : Z = Z / l;
114 : }
115 19 : }
116 :
117 : /**
118 : * Normalize the vector so that length is close to n.
119 : * If length is 0, does nothing.
120 : */
121 7 : void Normalize(fixed n)
122 : {
123 7 : fixed l = Length();
124 7 : if (!l.IsZero())
125 : {
126 6 : X = X.MulDiv(n, l);
127 6 : Y = Y.MulDiv(n, l);
128 6 : Z = Z.MulDiv(n, l);
129 : }
130 7 : }
131 :
132 : /**
133 : * Compute the cross product of this vector with another.
134 : */
135 13 : CFixedVector3D Cross(const CFixedVector3D& v)
136 : {
137 13 : i64 y_vz = MUL_I64_I32_I32(Y.GetInternalValue(), v.Z.GetInternalValue());
138 13 : i64 z_vy = MUL_I64_I32_I32(Z.GetInternalValue(), v.Y.GetInternalValue());
139 13 : CheckSignedSubtractionOverflow(i64, y_vz, z_vy, L"Overflow in CFixedVector3D::Cross() part 1", L"Underflow in CFixedVector3D::Cross() part 1")
140 13 : i64 x = y_vz - z_vy;
141 13 : x >>= fixed::fract_bits;
142 :
143 13 : i64 z_vx = MUL_I64_I32_I32(Z.GetInternalValue(), v.X.GetInternalValue());
144 13 : i64 x_vz = MUL_I64_I32_I32(X.GetInternalValue(), v.Z.GetInternalValue());
145 13 : CheckSignedSubtractionOverflow(i64, z_vx, x_vz, L"Overflow in CFixedVector3D::Cross() part 2", L"Underflow in CFixedVector3D::Cross() part 2")
146 13 : i64 y = z_vx - x_vz;
147 13 : y >>= fixed::fract_bits;
148 :
149 13 : i64 x_vy = MUL_I64_I32_I32(X.GetInternalValue(), v.Y.GetInternalValue());
150 13 : i64 y_vx = MUL_I64_I32_I32(Y.GetInternalValue(), v.X.GetInternalValue());
151 13 : CheckSignedSubtractionOverflow(i64, x_vy, y_vx, L"Overflow in CFixedVector3D::Cross() part 3", L"Underflow in CFixedVector3D::Cross() part 3")
152 13 : i64 z = x_vy - y_vx;
153 13 : z >>= fixed::fract_bits;
154 :
155 13 : CheckCastOverflow(x, i32, L"Overflow in CFixedVector3D::Cross() part 4", L"Underflow in CFixedVector3D::Cross() part 4")
156 13 : CheckCastOverflow(y, i32, L"Overflow in CFixedVector3D::Cross() part 5", L"Underflow in CFixedVector3D::Cross() part 5")
157 13 : CheckCastOverflow(z, i32, L"Overflow in CFixedVector3D::Cross() part 6", L"Underflow in CFixedVector3D::Cross() part 6")
158 13 : CFixedVector3D ret;
159 13 : ret.X.SetInternalValue((i32)x);
160 13 : ret.Y.SetInternalValue((i32)y);
161 13 : ret.Z.SetInternalValue((i32)z);
162 13 : return ret;
163 : }
164 :
165 : /**
166 : * Compute the dot product of this vector with another.
167 : */
168 1 : fixed Dot(const CFixedVector3D& v)
169 : {
170 1 : i64 x = MUL_I64_I32_I32(X.GetInternalValue(), v.X.GetInternalValue());
171 1 : i64 y = MUL_I64_I32_I32(Y.GetInternalValue(), v.Y.GetInternalValue());
172 1 : i64 z = MUL_I64_I32_I32(Z.GetInternalValue(), v.Z.GetInternalValue());
173 1 : CheckSignedAdditionOverflow(i64, x, y, L"Overflow in CFixedVector3D::Dot() part 1", L"Underflow in CFixedVector3D::Dot() part 1")
174 1 : i64 t = x + y;
175 :
176 1 : CheckSignedAdditionOverflow(i64, t, z, L"Overflow in CFixedVector3D::Dot() part 2", L"Underflow in CFixedVector3D::Dot() part 2")
177 1 : i64 sum = t + z;
178 1 : sum >>= fixed::fract_bits;
179 1 : CheckCastOverflow(sum, i32, L"Overflow in CFixedVector3D::Dot() part 3", L"Underflow in CFixedVector3D::Dot() part 3")
180 :
181 1 : fixed ret;
182 1 : ret.SetInternalValue((i32)sum);
183 1 : return ret;
184 : }
185 : };
186 :
187 : #endif // INCLUDED_FIXED_VECTOR3D
|