1 struct vec;
2 struct vec4;
3
4 struct vec2
5 {
6 union
7 {
8 struct { float x, y; };
9 float v[2];
10 };
11
vec2vec212 vec2() {}
vec2vec213 vec2(float x, float y) : x(x), y(y) {}
14 explicit vec2(const vec &v);
15 explicit vec2(const vec4 &v);
16
17 float &operator[](int i) { return v[i]; }
18 float operator[](int i) const { return v[i]; }
19
20 bool operator==(const vec2 &o) const { return x == o.x && y == o.y; }
21 bool operator!=(const vec2 &o) const { return x != o.x || y != o.y; }
22
iszerovec223 bool iszero() const { return x==0 && y==0; }
dotvec224 float dot(const vec2 &o) const { return x*o.x + y*o.y; }
squaredlenvec225 float squaredlen() const { return dot(*this); }
magnitudevec226 float magnitude() const { return sqrtf(squaredlen()); }
normalizevec227 vec2 &normalize() { mul(1/magnitude()); return *this; }
crossvec228 float cross(const vec2 &o) const { return x*o.y - y*o.x; }
squaredistvec229 float squaredist(const vec2 &e) const { return vec2(*this).sub(e).squaredlen(); }
distvec230 float dist(const vec2 &e) const { return sqrtf(squaredist(e)); }
31
mulvec232 vec2 &mul(float f) { x *= f; y *= f; return *this; }
mulvec233 vec2 &mul(const vec2 &o) { x *= o.x; y *= o.y; return *this; }
squarevec234 vec2 &square() { mul(*this); return *this; }
divvec235 vec2 &div(float f) { x /= f; y /= f; return *this; }
divvec236 vec2 &div(const vec2 &o) { x /= o.x; y /= o.y; return *this; }
recipvec237 vec2 &recip() { x = 1/x; y = 1/y; return *this; }
addvec238 vec2 &add(float f) { x += f; y += f; return *this; }
addvec239 vec2 &add(const vec2 &o) { x += o.x; y += o.y; return *this; }
subvec240 vec2 &sub(float f) { x -= f; y -= f; return *this; }
subvec241 vec2 &sub(const vec2 &o) { x -= o.x; y -= o.y; return *this; }
negvec242 vec2 &neg() { x = -x; y = -y; return *this; }
minvec243 vec2 &min(const vec2 &o) { x = ::min(x, o.x); y = ::min(y, o.y); return *this; }
maxvec244 vec2 &max(const vec2 &o) { x = ::max(x, o.x); y = ::max(y, o.y); return *this; }
minvec245 vec2 &min(float f) { x = ::min(x, f); y = ::min(y, f); return *this; }
maxvec246 vec2 &max(float f) { x = ::max(x, f); y = ::max(y, f); return *this; }
absvec247 vec2 &abs() { x = fabs(x); y = fabs(y); return *this; }
clampvec248 vec2 &clamp(float l, float h) { x = ::clamp(x, l, h); y = ::clamp(y, l, h); return *this; }
reflectvec249 vec2 &reflect(const vec2 &n) { float k = 2*dot(n); x -= k*n.x; y -= k*n.y; return *this; }
lerpvec250 vec2 &lerp(const vec2 &b, float t) { x += (b.x-x)*t; y += (b.y-y)*t; return *this; }
lerpvec251 vec2 &lerp(const vec2 &a, const vec2 &b, float t) { x = a.x + (b.x-a.x)*t; y = a.y + (b.y-a.y)*t; return *this; }
avgvec252 vec2 &avg(const vec2 &b) { add(b); mul(0.5f); return *this; }
maddvec253 template<class B> vec2 &madd(const vec2 &a, const B &b) { return add(vec2(a).mul(b)); }
msubvec254 template<class B> vec2 &msub(const vec2 &a, const B &b) { return sub(vec2(a).mul(b)); }
55
rotate_around_zvec256 vec2 &rotate_around_z(float c, float s) { float rx = x, ry = y; x = c*rx-s*ry; y = c*ry+s*rx; return *this; }
rotate_around_zvec257 vec2 &rotate_around_z(float angle) { return rotate_around_z(cosf(angle), sinf(angle)); }
rotate_around_zvec258 vec2 &rotate_around_z(const vec2 &sc) { return rotate_around_z(sc.x, sc.y); }
59 };
60
htcmp(const vec2 & x,const vec2 & y)61 static inline bool htcmp(const vec2 &x, const vec2 &y)
62 {
63 return x == y;
64 }
65
hthash(const vec2 & k)66 static inline uint hthash(const vec2 &k)
67 {
68 union { uint i; float f; } x, y;
69 x.f = k.x; y.f = k.y;
70 uint v = x.i^y.i;
71 return v + (v>>12);
72 }
73
74 struct ivec;
75 struct usvec;
76 struct svec;
77
78 struct vec
79 {
80 union
81 {
82 struct { float x, y, z; };
83 struct { float r, g, b; };
84 float v[3];
85 };
86
vecvec87 vec() {}
vecvec88 explicit vec(int a) : x(a), y(a), z(a) {}
vecvec89 explicit vec(float a) : x(a), y(a), z(a) {}
vecvec90 vec(float a, float b, float c) : x(a), y(b), z(c) {}
vecvec91 explicit vec(int v[3]) : x(v[0]), y(v[1]), z(v[2]) {}
vecvec92 explicit vec(const float *v) : x(v[0]), y(v[1]), z(v[2]) {}
93 explicit vec(const vec2 &v, float z = 0) : x(v.x), y(v.y), z(z) {}
94 explicit vec(const vec4 &v);
95 explicit vec(const ivec &v);
96 explicit vec(const usvec &v);
97 explicit vec(const svec &v);
98
vecvec99 vec(float yaw, float pitch) : x(-sinf(yaw)*cosf(pitch)), y(cosf(yaw)*cosf(pitch)), z(sinf(pitch)) {}
100
101 float &operator[](int i) { return v[i]; }
102 float operator[](int i) const { return v[i]; }
103
setvec104 vec &set(int i, float f) { v[i] = f; return *this; }
105
106 bool operator==(const vec &o) const { return x == o.x && y == o.y && z == o.z; }
107 bool operator!=(const vec &o) const { return x != o.x || y != o.y || z != o.z; }
108
iszerovec109 bool iszero() const { return x==0 && y==0 && z==0; }
squaredlenvec110 float squaredlen() const { return x*x + y*y + z*z; }
dot2vec111 float dot2(const vec2 &o) const { return x*o.x + y*o.y; }
dot2vec112 float dot2(const vec &o) const { return x*o.x + y*o.y; }
dotvec113 float dot(const vec &o) const { return x*o.x + y*o.y + z*o.z; }
squaredotvec114 float squaredot(const vec &o) const { float k = dot(o); return k*k; }
absdotvec115 float absdot(const vec &o) const { return fabs(x*o.x) + fabs(y*o.y) + fabs(z*o.z); }
zdotvec116 float zdot(const vec &o) const { return z*o.z; }
mulvec117 vec &mul(const vec &o) { x *= o.x; y *= o.y; z *= o.z; return *this; }
mulvec118 vec &mul(float f) { x *= f; y *= f; z *= f; return *this; }
squarevec119 vec &square() { mul(*this); return *this; }
divvec120 vec &div(const vec &o) { x /= o.x; y /= o.y; z /= o.z; return *this; }
divvec121 vec &div(float f) { x /= f; y /= f; z /= f; return *this; }
recipvec122 vec &recip() { x = 1/x; y = 1/y; z = 1/z; return *this; }
addvec123 vec &add(const vec &o) { x += o.x; y += o.y; z += o.z; return *this; }
addvec124 vec &add(float f) { x += f; y += f; z += f; return *this; }
addzvec125 vec &addz(float f) { z += f; return *this; }
subvec126 vec &sub(const vec &o) { x -= o.x; y -= o.y; z -= o.z; return *this; }
subvec127 vec &sub(float f) { x -= f; y -= f; z -= f; return *this; }
subzvec128 vec &subz(float f) { z -= f; return *this; }
neg2vec129 vec &neg2() { x = -x; y = -y; return *this; }
negvec130 vec &neg() { x = -x; y = -y; z = -z; return *this; }
minvec131 vec &min(const vec &o) { x = ::min(x, o.x); y = ::min(y, o.y); z = ::min(z, o.z); return *this; }
maxvec132 vec &max(const vec &o) { x = ::max(x, o.x); y = ::max(y, o.y); z = ::max(z, o.z); return *this; }
minvec133 vec &min(float f) { x = ::min(x, f); y = ::min(y, f); z = ::min(z, f); return *this; }
maxvec134 vec &max(float f) { x = ::max(x, f); y = ::max(y, f); z = ::max(z, f); return *this; }
absvec135 vec &abs() { x = fabs(x); y = fabs(y); z = fabs(z); return *this; }
clampvec136 vec &clamp(float l, float h) { x = ::clamp(x, l, h); y = ::clamp(y, l, h); z = ::clamp(z, l, h); return *this; }
magnitude2vec137 float magnitude2() const { return sqrtf(dot2(*this)); }
magnitudevec138 float magnitude() const { return sqrtf(squaredlen()); }
normalizevec139 vec &normalize() { div(magnitude()); return *this; }
isnormalizedvec140 bool isnormalized() const { float m = squaredlen(); return (m>0.99f && m<1.01f); }
squaredistvec141 float squaredist(const vec &e) const { return vec(*this).sub(e).squaredlen(); }
distvec142 float dist(const vec &e) const { return sqrtf(squaredist(e)); }
distvec143 float dist(const vec &e, vec &t) const { t = *this; t.sub(e); return t.magnitude(); }
dist2vec144 float dist2(const vec &o) const { float dx = x-o.x, dy = y-o.y; return sqrtf(dx*dx + dy*dy); }
145 template<class T>
rejectvec146 bool reject(const T &o, float r) { return x>o.x+r || x<o.x-r || y>o.y+r || y<o.y-r; }
147 template<class A, class B>
crossvec148 vec &cross(const A &a, const B &b) { x = a.y*b.z-a.z*b.y; y = a.z*b.x-a.x*b.z; z = a.x*b.y-a.y*b.x; return *this; }
crossvec149 vec &cross(const vec &o, const vec &a, const vec &b) { return cross(vec(a).sub(o), vec(b).sub(o)); }
scalartriplevec150 float scalartriple(const vec &a, const vec &b) const { return x*(a.y*b.z-a.z*b.y) + y*(a.z*b.x-a.x*b.z) + z*(a.x*b.y-a.y*b.x); }
zscalartriplevec151 float zscalartriple(const vec &a, const vec &b) const { return z*(a.x*b.y-a.y*b.x); }
reflectzvec152 vec &reflectz(float rz) { z = 2*rz - z; return *this; }
reflectvec153 vec &reflect(const vec &n) { float k = 2*dot(n); x -= k*n.x; y -= k*n.y; z -= k*n.z; return *this; }
projectvec154 vec &project(const vec &n) { float k = dot(n); x -= k*n.x; y -= k*n.y; z -= k*n.z; return *this; }
projectxydirvec155 vec &projectxydir(const vec &n) { if(n.z) z = -(x*n.x/n.z + y*n.y/n.z); return *this; }
projectxyvec156 vec &projectxy(const vec &n)
157 {
158 float m = squaredlen(), k = dot(n);
159 projectxydir(n);
160 rescale(sqrtf(::max(m - k*k, 0.0f)));
161 return *this;
162 }
projectxyvec163 vec &projectxy(const vec &n, float threshold)
164 {
165 float m = squaredlen(), k = ::min(dot(n), threshold);
166 projectxydir(n);
167 rescale(sqrtf(::max(m - k*k, 0.0f)));
168 return *this;
169 }
lerpvec170 vec &lerp(const vec &b, float t) { x += (b.x-x)*t; y += (b.y-y)*t; z += (b.z-z)*t; return *this; }
lerpvec171 vec &lerp(const vec &a, const vec &b, float t) { x = a.x + (b.x-a.x)*t; y = a.y + (b.y-a.y)*t; z = a.z + (b.z-a.z)*t; return *this; }
avgvec172 vec &avg(const vec &b) { add(b); mul(0.5f); return *this; }
maddvec173 template<class B> vec &madd(const vec &a, const B &b) { return add(vec(a).mul(b)); }
msubvec174 template<class B> vec &msub(const vec &a, const B &b) { return sub(vec(a).mul(b)); }
175
rescalevec176 vec &rescale(float k)
177 {
178 float mag = magnitude();
179 if(mag > 1e-6f) mul(k / mag);
180 return *this;
181 }
182
rotate_around_zvec183 vec &rotate_around_z(float c, float s) { float rx = x, ry = y; x = c*rx-s*ry; y = c*ry+s*rx; return *this; }
rotate_around_xvec184 vec &rotate_around_x(float c, float s) { float ry = y, rz = z; y = c*ry-s*rz; z = c*rz+s*ry; return *this; }
rotate_around_yvec185 vec &rotate_around_y(float c, float s) { float rx = x, rz = z; x = c*rx+s*rz; z = c*rz-s*rx; return *this; }
186
rotate_around_zvec187 vec &rotate_around_z(float angle) { return rotate_around_z(cosf(angle), sinf(angle)); }
rotate_around_xvec188 vec &rotate_around_x(float angle) { return rotate_around_x(cosf(angle), sinf(angle)); }
rotate_around_yvec189 vec &rotate_around_y(float angle) { return rotate_around_y(cosf(angle), sinf(angle)); }
190
rotate_around_zvec191 vec &rotate_around_z(const vec2 &sc) { return rotate_around_z(sc.x, sc.y); }
rotate_around_xvec192 vec &rotate_around_x(const vec2 &sc) { return rotate_around_x(sc.x, sc.y); }
rotate_around_yvec193 vec &rotate_around_y(const vec2 &sc) { return rotate_around_y(sc.x, sc.y); }
194
rotatevec195 vec &rotate(float c, float s, const vec &d)
196 {
197 *this = vec(x*(d.x*d.x*(1-c)+c) + y*(d.x*d.y*(1-c)-d.z*s) + z*(d.x*d.z*(1-c)+d.y*s),
198 x*(d.y*d.x*(1-c)+d.z*s) + y*(d.y*d.y*(1-c)+c) + z*(d.y*d.z*(1-c)-d.x*s),
199 x*(d.x*d.z*(1-c)-d.y*s) + y*(d.y*d.z*(1-c)+d.x*s) + z*(d.z*d.z*(1-c)+c));
200 return *this;
201 }
rotatevec202 vec &rotate(float angle, const vec &d) { return rotate(cosf(angle), sinf(angle), d); }
rotatevec203 vec &rotate(const vec2 &sc, const vec &d) { return rotate(sc.x, sc.y, d); }
204
orthogonalvec205 void orthogonal(const vec &d)
206 {
207 int i = fabs(d.x) > fabs(d.y) ? (fabs(d.x) > fabs(d.z) ? 0 : 2) : (fabs(d.y) > fabs(d.z) ? 1 : 2);
208 v[i] = d[(i+1)%3];
209 v[(i+1)%3] = -d[i];
210 v[(i+2)%3] = 0;
211 }
212
orthonormalizevec213 void orthonormalize(vec &s, vec &t) const
214 {
215 s.project(*this);
216 t.project(*this).project(s);
217 }
218
219 template<class T>
insidebbvec220 bool insidebb(const T &bbmin, const T &bbmax) const
221 {
222 return x >= bbmin.x && x <= bbmax.x && y >= bbmin.y && y <= bbmax.y && z >= bbmin.z && z <= bbmax.z;
223 }
224
225 template<class T, class U>
insidebbvec226 bool insidebb(const T &o, U size) const
227 {
228 return x >= o.x && x <= o.x + size && y >= o.y && y <= o.y + size && z >= o.z && z <= o.z + size;
229 }
230
dist_to_bbvec231 template<class T> float dist_to_bb(const T &min, const T &max) const
232 {
233 float sqrdist = 0;
234 loopi(3)
235 {
236 if (v[i] < min[i]) { float delta = v[i]-min[i]; sqrdist += delta*delta; }
237 else if(v[i] > max[i]) { float delta = max[i]-v[i]; sqrdist += delta*delta; }
238 }
239 return sqrtf(sqrdist);
240 }
241
dist_to_bbvec242 template<class T, class S> float dist_to_bb(const T &o, S size) const
243 {
244 return dist_to_bb(o, T(o).add(size));
245 }
246
project_bbvec247 template<class T> float project_bb(const T &min, const T &max) const
248 {
249 return x*(x < 0 ? max.x : min.x) + y*(y < 0 ? max.y : min.y) + z*(z < 0 ? max.z : min.z);
250 }
251
hexcolorvec252 static vec hexcolor(int color)
253 {
254 return vec(((color>>16)&0xFF)*(1.0f/255.0f), ((color>>8)&0xFF)*(1.0f/255.0f), (color&0xFF)*(1.0f/255.0f));
255 }
256
tohexcolorvec257 int tohexcolor() const { return (int(::clamp(r, 0.0f, 1.0f)*255)<<16)|(int(::clamp(g, 0.0f, 1.0f)*255)<<8)|int(::clamp(b, 0.0f, 1.0f)*255); }
258 };
259
vec2(const vec & v)260 inline vec2::vec2(const vec &v) : x(v.x), y(v.y) {}
261
htcmp(const vec & x,const vec & y)262 static inline bool htcmp(const vec &x, const vec &y)
263 {
264 return x == y;
265 }
266
hthash(const vec & k)267 static inline uint hthash(const vec &k)
268 {
269 union { uint i; float f; } x, y, z;
270 x.f = k.x; y.f = k.y; z.f = k.z;
271 uint v = x.i^y.i^z.i;
272 return v + (v>>12);
273 }
274
275 struct vec4
276 {
277 union
278 {
279 struct { float x, y, z, w; };
280 struct { float r, g, b, a; };
281 float v[4];
282 };
283
vec4vec4284 vec4() {}
285 explicit vec4(const vec &p, float w = 0) : x(p.x), y(p.y), z(p.z), w(w) {}
vec4vec4286 vec4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}
vec4vec4287 explicit vec4(const float *v) : x(v[0]), y(v[1]), z(v[2]), w(v[3]) {}
288
289 float &operator[](int i) { return v[i]; }
290 float operator[](int i) const { return v[i]; }
291
292 bool operator==(const vec4 &o) const { return x == o.x && y == o.y && z == o.z && w == o.w; }
293 bool operator!=(const vec4 &o) const { return x != o.x || y != o.y || z != o.z || w != o.w; }
294
dot3vec4295 float dot3(const vec4 &o) const { return x*o.x + y*o.y + z*o.z; }
dot3vec4296 float dot3(const vec &o) const { return x*o.x + y*o.y + z*o.z; }
dotvec4297 float dot(const vec4 &o) const { return dot3(o) + w*o.w; }
dotvec4298 float dot(const vec &o) const { return x*o.x + y*o.y + z*o.z + w; }
squaredlenvec4299 float squaredlen() const { return dot(*this); }
magnitudevec4300 float magnitude() const { return sqrtf(squaredlen()); }
magnitude3vec4301 float magnitude3() const { return sqrtf(dot3(*this)); }
normalizevec4302 vec4 &normalize() { mul(1/magnitude()); return *this; }
303
lerpvec4304 vec4 &lerp(const vec4 &b, float t)
305 {
306 x += (b.x-x)*t;
307 y += (b.y-y)*t;
308 z += (b.z-z)*t;
309 w += (b.w-w)*t;
310 return *this;
311 }
lerpvec4312 vec4 &lerp(const vec4 &a, const vec4 &b, float t)
313 {
314 x = a.x+(b.x-a.x)*t;
315 y = a.y+(b.y-a.y)*t;
316 z = a.z+(b.z-a.z)*t;
317 w = a.w+(b.w-a.w)*t;
318 return *this;
319 }
avgvec4320 vec4 &avg(const vec4 &b) { add(b); mul(0.5f); return *this; }
maddvec4321 template<class B> vec4 &madd(const vec4 &a, const B &b) { return add(vec4(a).mul(b)); }
msubvec4322 template<class B> vec4 &msub(const vec4 &a, const B &b) { return sub(vec4(a).mul(b)); }
323
mul3vec4324 vec4 &mul3(float f) { x *= f; y *= f; z *= f; return *this; }
mulvec4325 vec4 &mul(float f) { mul3(f); w *= f; return *this; }
mulvec4326 vec4 &mul(const vec4 &o) { x *= o.x; y *= o.y; z *= o.z; w *= o.w; return *this; }
mulvec4327 vec4 &mul(const vec &o) { x *= o.x; y *= o.y; z *= o.z; return *this; }
squarevec4328 vec4 &square() { mul(*this); return *this; }
div3vec4329 vec4 &div3(float f) { x /= f; y /= f; z /= f; return *this; }
divvec4330 vec4 &div(float f) { div3(f); w /= f; return *this; }
divvec4331 vec4 &div(const vec4 &o) { x /= o.x; y /= o.y; z /= o.z; w /= o.w; return *this; }
divvec4332 vec4 &div(const vec &o) { x /= o.x; y /= o.y; z /= o.z; return *this; }
recipvec4333 vec4 &recip() { x = 1/x; y = 1/y; z = 1/z; w = 1/w; return *this; }
addvec4334 vec4 &add(const vec4 &o) { x += o.x; y += o.y; z += o.z; w += o.w; return *this; }
addvec4335 vec4 &add(const vec &o) { x += o.x; y += o.y; z += o.z; return *this; }
add3vec4336 vec4 &add3(float f) { x += f; y += f; z += f; return *this; }
addvec4337 vec4 &add(float f) { add3(f); w += f; return *this; }
addwvec4338 vec4 &addw(float f) { w += f; return *this; }
subvec4339 vec4 &sub(const vec4 &o) { x -= o.x; y -= o.y; z -= o.z; w -= o.w; return *this; }
subvec4340 vec4 &sub(const vec &o) { x -= o.x; y -= o.y; z -= o.z; return *this; }
sub3vec4341 vec4 &sub3(float f) { x -= f; y -= f; z -= f; return *this; }
subvec4342 vec4 &sub(float f) { sub3(f); w -= f; return *this; }
subwvec4343 vec4 &subw(float f) { w -= f; return *this; }
neg3vec4344 vec4 &neg3() { x = -x; y = -y; z = -z; return *this; }
negvec4345 vec4 &neg() { neg3(); w = -w; return *this; }
clampvec4346 vec4 &clamp(float l, float h) { x = ::clamp(x, l, h); y = ::clamp(y, l, h); z = ::clamp(z, l, h); w = ::clamp(w, l, h); return *this; }
347
348 template<class A, class B>
crossvec4349 vec4 &cross(const A &a, const B &b) { x = a.y*b.z-a.z*b.y; y = a.z*b.x-a.x*b.z; z = a.x*b.y-a.y*b.x; return *this; }
crossvec4350 vec4 &cross(const vec &o, const vec &a, const vec &b) { return cross(vec(a).sub(o), vec(b).sub(o)); }
351
setxyzvec4352 void setxyz(const vec &v) { x = v.x; y = v.y; z = v.z; }
353
rotate_around_zvec4354 vec4 &rotate_around_z(float c, float s) { float rx = x, ry = y; x = c*rx-s*ry; y = c*ry+s*rx; return *this; }
rotate_around_xvec4355 vec4 &rotate_around_x(float c, float s) { float ry = y, rz = z; y = c*ry-s*rz; z = c*rz+s*ry; return *this; }
rotate_around_yvec4356 vec4 &rotate_around_y(float c, float s) { float rx = x, rz = z; x = c*rx-s*rz; z = c*rz+s*rx; return *this; }
357
rotate_around_zvec4358 vec4 &rotate_around_z(float angle) { return rotate_around_z(cosf(angle), sinf(angle)); }
rotate_around_xvec4359 vec4 &rotate_around_x(float angle) { return rotate_around_x(cosf(angle), sinf(angle)); }
rotate_around_yvec4360 vec4 &rotate_around_y(float angle) { return rotate_around_y(cosf(angle), sinf(angle)); }
361
rotate_around_zvec4362 vec4 &rotate_around_z(const vec2 &sc) { return rotate_around_z(sc.x, sc.y); }
rotate_around_xvec4363 vec4 &rotate_around_x(const vec2 &sc) { return rotate_around_x(sc.x, sc.y); }
rotate_around_yvec4364 vec4 &rotate_around_y(const vec2 &sc) { return rotate_around_y(sc.x, sc.y); }
365 };
366
vec2(const vec4 & v)367 inline vec2::vec2(const vec4 &v) : x(v.x), y(v.y) {}
vec(const vec4 & v)368 inline vec::vec(const vec4 &v) : x(v.x), y(v.y), z(v.z) {}
369
370 struct matrix3;
371 struct matrix4x3;
372 struct matrix4;
373
374 struct quat : vec4
375 {
quatquat376 quat() {}
quatquat377 quat(float x, float y, float z, float w) : vec4(x, y, z, w) {}
quatquat378 quat(const vec &axis, float angle)
379 {
380 w = cosf(angle/2);
381 float s = sinf(angle/2);
382 x = s*axis.x;
383 y = s*axis.y;
384 z = s*axis.z;
385 }
quatquat386 quat(const vec &u, const vec &v)
387 {
388 w = sqrtf(u.squaredlen() * v.squaredlen()) + u.dot(v);
389 cross(u, v);
390 normalize();
391 }
quatquat392 explicit quat(const vec &v)
393 {
394 x = v.x;
395 y = v.y;
396 z = v.z;
397 restorew();
398 }
quatquat399 explicit quat(const matrix3 &m) { convertmatrix(m); }
quatquat400 explicit quat(const matrix4x3 &m) { convertmatrix(m); }
quatquat401 explicit quat(const matrix4 &m) { convertmatrix(m); }
402
restorewquat403 void restorew() { w = 1.0f-x*x-y*y-z*z; w = w<0 ? 0 : -sqrtf(w); }
404
addquat405 quat &add(const vec4 &o) { vec4::add(o); return *this; }
subquat406 quat &sub(const vec4 &o) { vec4::sub(o); return *this; }
mulquat407 quat &mul(float k) { vec4::mul(k); return *this; }
maddquat408 template<class B> quat &madd(const vec4 &a, const B &b) { return add(vec4(a).mul(b)); }
msubquat409 template<class B> quat &msub(const vec4 &a, const B &b) { return sub(vec4(a).mul(b)); }
410
mulquat411 quat &mul(const quat &p, const quat &o)
412 {
413 x = p.w*o.x + p.x*o.w + p.y*o.z - p.z*o.y;
414 y = p.w*o.y - p.x*o.z + p.y*o.w + p.z*o.x;
415 z = p.w*o.z + p.x*o.y - p.y*o.x + p.z*o.w;
416 w = p.w*o.w - p.x*o.x - p.y*o.y - p.z*o.z;
417 return *this;
418 }
mulquat419 quat &mul(const quat &o) { return mul(quat(*this), o); }
420
invertquat421 quat &invert() { neg3(); return *this; }
422
normalizequat423 quat &normalize() { vec4::normalize(); return *this; }
424
calcangleaxisquat425 void calcangleaxis(float &angle, vec &axis) const
426 {
427 float rr = dot3(*this);
428 if(rr>0)
429 {
430 angle = 2*acosf(w);
431 axis = vec(x, y, z).mul(1/rr);
432 }
433 else { angle = 0; axis = vec(0, 0, 1); }
434 }
435
calcanglesquat436 vec calcangles() const
437 {
438 vec4 qq = vec4(*this).square();
439 float rr = qq.x + qq.y + qq.z + qq.w,
440 t = x*y + z*w;
441 if(fabs(t) > 0.49999f*rr) return t < 0 ? vec(-2*atan2f(x, w), -M_PI/2, 0) : vec(2*atan2f(x, w), M_PI/2, 0);
442 return vec(atan2f(2*(y*w - x*z), qq.x - qq.y - qq.z + qq.w),
443 asinf(2*t/rr),
444 atan2f(2*(x*w - y*z), -qq.x + qq.y - qq.z + qq.w));
445 }
446
rotatequat447 vec rotate(const vec &v) const
448 {
449 return vec().cross(*this, vec().cross(*this, v).madd(v, w)).mul(2).add(v);
450 }
451
invertedrotatequat452 vec invertedrotate(const vec &v) const
453 {
454 return vec().cross(*this, vec().cross(*this, v).msub(v, w)).mul(2).add(v);
455 }
456
457 template<class M>
convertmatrixquat458 void convertmatrix(const M &m)
459 {
460 float trace = m.a.x + m.b.y + m.c.z;
461 if(trace>0)
462 {
463 float r = sqrtf(1 + trace), inv = 0.5f/r;
464 w = 0.5f*r;
465 x = (m.b.z - m.c.y)*inv;
466 y = (m.c.x - m.a.z)*inv;
467 z = (m.a.y - m.b.x)*inv;
468 }
469 else if(m.a.x > m.b.y && m.a.x > m.c.z)
470 {
471 float r = sqrtf(1 + m.a.x - m.b.y - m.c.z), inv = 0.5f/r;
472 x = 0.5f*r;
473 y = (m.a.y + m.b.x)*inv;
474 z = (m.c.x + m.a.z)*inv;
475 w = (m.b.z - m.c.y)*inv;
476 }
477 else if(m.b.y > m.c.z)
478 {
479 float r = sqrtf(1 + m.b.y - m.a.x - m.c.z), inv = 0.5f/r;
480 x = (m.a.y + m.b.x)*inv;
481 y = 0.5f*r;
482 z = (m.b.z + m.c.y)*inv;
483 w = (m.c.x - m.a.z)*inv;
484 }
485 else
486 {
487 float r = sqrtf(1 + m.c.z - m.a.x - m.b.y), inv = 0.5f/r;
488 x = (m.c.x + m.a.z)*inv;
489 y = (m.b.z + m.c.y)*inv;
490 z = 0.5f*r;
491 w = (m.a.y - m.b.x)*inv;
492 }
493 }
494 };
495
496 struct dualquat
497 {
498 quat real, dual;
499
dualquatdualquat500 dualquat() {}
dualquatdualquat501 dualquat(const quat &q, const vec &p)
502 : real(q),
503 dual(0.5f*( p.x*q.w + p.y*q.z - p.z*q.y),
504 0.5f*(-p.x*q.z + p.y*q.w + p.z*q.x),
505 0.5f*( p.x*q.y - p.y*q.x + p.z*q.w),
506 -0.5f*( p.x*q.x + p.y*q.y + p.z*q.z))
507 {
508 }
dualquatdualquat509 explicit dualquat(const quat &q) : real(q), dual(0, 0, 0, 0) {}
510 explicit dualquat(const matrix4x3 &m);
511
muldualquat512 dualquat &mul(float k) { real.mul(k); dual.mul(k); return *this; }
adddualquat513 dualquat &add(const dualquat &d) { real.add(d.real); dual.add(d.dual); return *this; }
514
lerpdualquat515 dualquat &lerp(const dualquat &to, float t)
516 {
517 float k = real.dot(to.real) < 0 ? -t : t;
518 real.mul(1-t).madd(to.real, k);
519 dual.mul(1-t).madd(to.dual, k);
520 return *this;
521 }
lerpdualquat522 dualquat &lerp(const dualquat &from, const dualquat &to, float t)
523 {
524 float k = from.real.dot(to.real) < 0 ? -t : t;
525 (real = from.real).mul(1-t).madd(to.real, k);
526 (dual = from.dual).mul(1-t).madd(to.dual, k);
527 return *this;
528 }
529
invertdualquat530 dualquat &invert()
531 {
532 real.invert();
533 dual.invert();
534 dual.msub(real, 2*real.dot(dual));
535 return *this;
536 }
537
muldualquat538 void mul(const dualquat &p, const dualquat &o)
539 {
540 real.mul(p.real, o.real);
541 dual.mul(p.real, o.dual).add(quat().mul(p.dual, o.real));
542 }
muldualquat543 void mul(const dualquat &o) { mul(dualquat(*this), o); }
544
mulorientdualquat545 void mulorient(const quat &q)
546 {
547 real.mul(q, quat(real));
548 dual.mul(quat(q).invert(), quat(dual));
549 }
550
mulorientdualquat551 void mulorient(const quat &q, const dualquat &base)
552 {
553 quat trans;
554 trans.mul(base.dual, quat(base.real).invert());
555 dual.mul(quat(q).invert(), quat().mul(real, trans).add(dual));
556
557 real.mul(q, quat(real));
558 dual.add(quat().mul(real, trans.invert())).msub(real, 2*base.real.dot(base.dual));
559 }
560
normalizedualquat561 void normalize()
562 {
563 float invlen = 1/real.magnitude();
564 real.mul(invlen);
565 dual.mul(invlen);
566 }
567
translatedualquat568 void translate(const vec &p)
569 {
570 dual.x += 0.5f*( p.x*real.w + p.y*real.z - p.z*real.y);
571 dual.y += 0.5f*(-p.x*real.z + p.y*real.w + p.z*real.x);
572 dual.z += 0.5f*( p.x*real.y - p.y*real.x + p.z*real.w);
573 dual.w += -0.5f*( p.x*real.x + p.y*real.y + p.z*real.z);
574 }
575
scaledualquat576 void scale(float k)
577 {
578 dual.mul(k);
579 }
580
fixantipodaldualquat581 void fixantipodal(const dualquat &d)
582 {
583 if(real.dot(d.real) < 0)
584 {
585 real.neg();
586 dual.neg();
587 }
588 }
589
accumulatedualquat590 void accumulate(const dualquat &d, float k)
591 {
592 if(real.dot(d.real) < 0) k = -k;
593 real.madd(d.real, k);
594 dual.madd(d.dual, k);
595 }
596
transformdualquat597 vec transform(const vec &v) const
598 {
599 return vec().cross(real, vec().cross(real, v).madd(v, real.w).add(vec(dual))).madd(vec(dual), real.w).msub(vec(real), dual.w).mul(2).add(v);
600 }
601
transformdualquat602 quat transform(const quat &q) const
603 {
604 return quat().mul(real, q);
605 }
606
transposedtransformdualquat607 vec transposedtransform(const vec &v) const
608 {
609 return dualquat(*this).invert().transform(v);
610 }
611
transformnormaldualquat612 vec transformnormal(const vec &v) const
613 {
614 return real.rotate(v);
615 }
616
transposedtransformnormaldualquat617 vec transposedtransformnormal(const vec &v) const
618 {
619 return real.invertedrotate(v);
620 }
621
gettranslationdualquat622 vec gettranslation() const
623 {
624 return vec().cross(real, dual).madd(vec(dual), real.w).msub(vec(real), dual.w).mul(2);
625 }
626 };
627
628 struct matrix3
629 {
630 vec a, b, c;
631
matrix3matrix3632 matrix3() {}
matrix3matrix3633 matrix3(const vec &a, const vec &b, const vec &c) : a(a), b(b), c(c) {}
matrix3matrix3634 explicit matrix3(float angle, const vec &axis) { rotate(angle, axis); }
matrix3matrix3635 explicit matrix3(const quat &q)
636 {
637 float x = q.x, y = q.y, z = q.z, w = q.w,
638 tx = 2*x, ty = 2*y, tz = 2*z,
639 txx = tx*x, tyy = ty*y, tzz = tz*z,
640 txy = tx*y, txz = tx*z, tyz = ty*z,
641 twx = w*tx, twy = w*ty, twz = w*tz;
642 a = vec(1 - (tyy + tzz), txy + twz, txz - twy);
643 b = vec(txy - twz, 1 - (txx + tzz), tyz + twx);
644 c = vec(txz + twy, tyz - twx, 1 - (txx + tyy));
645 }
646 explicit matrix3(const matrix4x3 &m);
647 explicit matrix3(const matrix4 &m);
648
mulmatrix3649 void mul(const matrix3 &m, const matrix3 &n)
650 {
651 a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
652 b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
653 c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
654 }
mulmatrix3655 void mul(const matrix3 &n) { mul(matrix3(*this), n); }
656
multransposematrix3657 void multranspose(const matrix3 &m, const matrix3 &n)
658 {
659 a = vec(m.a).mul(n.a.x).madd(m.b, n.b.x).madd(m.c, n.c.x);
660 b = vec(m.a).mul(n.a.y).madd(m.b, m.b.y).madd(m.c, n.c.y);
661 c = vec(m.a).mul(n.a.z).madd(m.b, n.b.z).madd(m.c, n.c.z);
662 }
multransposematrix3663 void multranspose(const matrix3 &n) { multranspose(matrix3(*this), n); }
664
transposemulmatrix3665 void transposemul(const matrix3 &m, const matrix3 &n)
666 {
667 a = vec(m.a.dot(n.a), m.b.dot(n.a), m.c.dot(n.a));
668 b = vec(m.a.dot(n.b), m.b.dot(n.b), m.c.dot(n.b));
669 c = vec(m.a.dot(n.c), m.b.dot(n.c), m.c.dot(n.c));
670 }
transposemulmatrix3671 void transposemul(const matrix3 &n) { transposemul(matrix3(*this), n); }
672
transposematrix3673 void transpose()
674 {
675 swap(a.y, b.x); swap(a.z, c.x);
676 swap(b.z, c.y);
677 }
678
679 template<class M>
transposematrix3680 void transpose(const M &m)
681 {
682 a = vec(m.a.x, m.b.x, m.c.x);
683 b = vec(m.a.y, m.b.y, m.c.y);
684 c = vec(m.a.z, m.b.z, m.c.z);
685 }
686
invertmatrix3687 void invert(const matrix3 &o)
688 {
689 vec unscale(1/o.a.squaredlen(), 1/o.b.squaredlen(), 1/o.c.squaredlen());
690 transpose(o);
691 a.mul(unscale);
692 b.mul(unscale);
693 c.mul(unscale);
694 }
invertmatrix3695 void invert() { invert(matrix3(*this)); }
696
normalizematrix3697 void normalize()
698 {
699 a.normalize();
700 b.normalize();
701 c.normalize();
702 }
703
scalematrix3704 void scale(float k)
705 {
706 a.mul(k);
707 b.mul(k);
708 c.mul(k);
709 }
710
rotatematrix3711 void rotate(float angle, const vec &axis)
712 {
713 rotate(cosf(angle), sinf(angle), axis);
714 }
715
rotatematrix3716 void rotate(float ck, float sk, const vec &axis)
717 {
718 a = vec(axis.x*axis.x*(1-ck)+ck, axis.x*axis.y*(1-ck)+axis.z*sk, axis.x*axis.z*(1-ck)-axis.y*sk);
719 b = vec(axis.x*axis.y*(1-ck)-axis.z*sk, axis.y*axis.y*(1-ck)+ck, axis.y*axis.z*(1-ck)+axis.x*sk);
720 c = vec(axis.x*axis.z*(1-ck)+axis.y*sk, axis.y*axis.z*(1-ck)-axis.x*sk, axis.z*axis.z*(1-ck)+ck);
721 }
722
setyawmatrix3723 void setyaw(float ck, float sk)
724 {
725 a = vec(ck, sk, 0);
726 b = vec(-sk, ck, 0);
727 c = vec(0, 0, 1);
728 }
729
setyawmatrix3730 void setyaw(float angle)
731 {
732 setyaw(cosf(angle), sinf(angle));
733 }
734
tracematrix3735 float trace() const { return a.x + b.y + c.z; }
736
737 bool calcangleaxis(float tr, float &angle, vec &axis, float threshold = 1e-16f) const
738 {
739 if(tr <= -1)
740 {
741 if(a.x >= b.y && a.x >= c.z)
742 {
743 float r = 1 + a.x - b.y - c.z;
744 if(r <= threshold) return false;
745 r = sqrtf(r);
746 axis.x = 0.5f*r;
747 axis.y = b.x/r;
748 axis.z = c.x/r;
749 }
750 else if(b.y >= c.z)
751 {
752 float r = 1 + b.y - a.x - c.z;
753 if(r <= threshold) return false;
754 r = sqrtf(r);
755 axis.y = 0.5f*r;
756 axis.x = b.x/r;
757 axis.z = c.y/r;
758 }
759 else
760 {
761 float r = 1 + b.y - a.x - c.z;
762 if(r <= threshold) return false;
763 r = sqrtf(r);
764 axis.z = 0.5f*r;
765 axis.x = c.x/r;
766 axis.y = c.y/r;
767 }
768 angle = M_PI;
769 }
770 else if(tr >= 3)
771 {
772 axis = vec(0, 0, 1);
773 angle = 0;
774 }
775 else
776 {
777 axis = vec(b.z - c.y, c.x - a.z, a.y - b.x);
778 float r = axis.squaredlen();
779 if(r <= threshold) return false;
780 axis.mul(1/sqrtf(r));
781 angle = acosf(0.5f*(tr - 1));
782 }
783 return true;
784 }
785
786 bool calcangleaxis(float &angle, vec &axis, float threshold = 1e-16f) const { return calcangleaxis(trace(), angle, axis, threshold); }
787
transformmatrix3788 vec transform(const vec &o) const
789 {
790 return vec(a).mul(o.x).madd(b, o.y).madd(c, o.z);
791 }
transposedtransformmatrix3792 vec transposedtransform(const vec &o) const { return vec(a.dot(o), b.dot(o), c.dot(o)); }
abstransformmatrix3793 vec abstransform(const vec &o) const
794 {
795 return vec(a).mul(o.x).abs().add(vec(b).mul(o.y).abs()).add(vec(c).mul(o.z).abs());
796 }
abstransposedtransformmatrix3797 vec abstransposedtransform(const vec &o) const
798 {
799 return vec(a.absdot(o), b.absdot(o), c.absdot(o));
800 }
801
identitymatrix3802 void identity()
803 {
804 a = vec(1, 0, 0);
805 b = vec(0, 1, 0);
806 c = vec(0, 0, 1);
807 }
808
rotate_around_xmatrix3809 void rotate_around_x(float ck, float sk)
810 {
811 vec rb = vec(b).mul(ck).madd(c, sk),
812 rc = vec(c).mul(ck).msub(b, sk);
813 b = rb;
814 c = rc;
815 }
rotate_around_xmatrix3816 void rotate_around_x(float angle) { rotate_around_x(cosf(angle), sinf(angle)); }
rotate_around_xmatrix3817 void rotate_around_x(const vec2 &sc) { rotate_around_x(sc.x, sc.y); }
818
rotate_around_ymatrix3819 void rotate_around_y(float ck, float sk)
820 {
821 vec rc = vec(c).mul(ck).madd(a, sk),
822 ra = vec(a).mul(ck).msub(c, sk);
823 c = rc;
824 a = ra;
825 }
rotate_around_ymatrix3826 void rotate_around_y(float angle) { rotate_around_y(cosf(angle), sinf(angle)); }
rotate_around_ymatrix3827 void rotate_around_y(const vec2 &sc) { rotate_around_y(sc.x, sc.y); }
828
rotate_around_zmatrix3829 void rotate_around_z(float ck, float sk)
830 {
831 vec ra = vec(a).mul(ck).madd(b, sk),
832 rb = vec(b).mul(ck).msub(a, sk);
833 a = ra;
834 b = rb;
835 }
rotate_around_zmatrix3836 void rotate_around_z(float angle) { rotate_around_z(cosf(angle), sinf(angle)); }
rotate_around_zmatrix3837 void rotate_around_z(const vec2 &sc) { rotate_around_z(sc.x, sc.y); }
838
transformmatrix3839 vec transform(const vec2 &o) { return vec(a).mul(o.x).madd(b, o.y); }
transposedtransformmatrix3840 vec transposedtransform(const vec2 &o) const { return vec(a.dot2(o), b.dot2(o), c.dot2(o)); }
841
rowxmatrix3842 vec rowx() const { return vec(a.x, b.x, c.x); }
rowymatrix3843 vec rowy() const { return vec(a.y, b.y, c.y); }
rowzmatrix3844 vec rowz() const { return vec(a.z, b.z, c.z); }
845 };
846
847 struct matrix4x3
848 {
849 vec a, b, c, d;
850
matrix4x3matrix4x3851 matrix4x3() {}
matrix4x3matrix4x3852 matrix4x3(const vec &a, const vec &b, const vec &c, const vec &d) : a(a), b(b), c(c), d(d) {}
matrix4x3matrix4x3853 matrix4x3(const matrix3 &rot, const vec &trans) : a(rot.a), b(rot.b), c(rot.c), d(trans) {}
matrix4x3matrix4x3854 matrix4x3(const dualquat &dq)
855 {
856 vec4 r = vec4(dq.real).mul(1/dq.real.squaredlen()), rr = vec4(r).mul(dq.real);
857 r.mul(2);
858 float xy = r.x*dq.real.y, xz = r.x*dq.real.z, yz = r.y*dq.real.z,
859 wx = r.w*dq.real.x, wy = r.w*dq.real.y, wz = r.w*dq.real.z;
860 a = vec(rr.w + rr.x - rr.y - rr.z, xy + wz, xz - wy);
861 b = vec(xy - wz, rr.w + rr.y - rr.x - rr.z, yz + wx);
862 c = vec(xz + wy, yz - wx, rr.w + rr.z - rr.x - rr.y);
863 d = vec(-(dq.dual.w*r.x - dq.dual.x*r.w + dq.dual.y*r.z - dq.dual.z*r.y),
864 -(dq.dual.w*r.y - dq.dual.x*r.z - dq.dual.y*r.w + dq.dual.z*r.x),
865 -(dq.dual.w*r.z + dq.dual.x*r.y - dq.dual.y*r.x - dq.dual.z*r.w));
866
867 }
868 explicit matrix4x3(const matrix4 &m);
869
mulmatrix4x3870 void mul(float k)
871 {
872 a.mul(k);
873 b.mul(k);
874 c.mul(k);
875 d.mul(k);
876 }
877
setscalematrix4x3878 void setscale(float x, float y, float z) { a.x = x; b.y = y; c.z = z; }
setscalematrix4x3879 void setscale(const vec &v) { setscale(v.x, v.y, v.z); }
setscalematrix4x3880 void setscale(float n) { setscale(n, n, n); }
881
scalematrix4x3882 void scale(float x, float y, float z)
883 {
884 a.mul(x);
885 b.mul(y);
886 c.mul(z);
887 }
scalematrix4x3888 void scale(const vec &v) { scale(v.x, v.y, v.z); }
scalematrix4x3889 void scale(float n) { scale(n, n, n); }
890
settranslationmatrix4x3891 void settranslation(const vec &p) { d = p; }
settranslationmatrix4x3892 void settranslation(float x, float y, float z) { d = vec(x, y, z); }
893
translatematrix4x3894 void translate(const vec &p) { d.madd(a, p.x).madd(b, p.y).madd(c, p.z); }
translatematrix4x3895 void translate(float x, float y, float z) { translate(vec(x, y, z)); }
translatematrix4x3896 void translate(const vec &p, float scale) { translate(vec(p).mul(scale)); }
897
accumulatematrix4x3898 void accumulate(const matrix4x3 &m, float k)
899 {
900 a.madd(m.a, k);
901 b.madd(m.b, k);
902 c.madd(m.c, k);
903 d.madd(m.d, k);
904 }
905
normalizematrix4x3906 void normalize()
907 {
908 a.normalize();
909 b.normalize();
910 c.normalize();
911 }
912
lerpmatrix4x3913 void lerp(const matrix4x3 &to, float t)
914 {
915 a.lerp(to.a, t);
916 b.lerp(to.b, t);
917 c.lerp(to.c, t);
918 d.lerp(to.d, t);
919 }
lerpmatrix4x3920 void lerp(const matrix4x3 &from, const matrix4x3 &to, float t)
921 {
922 a.lerp(from.a, to.a, t);
923 b.lerp(from.b, to.b, t);
924 c.lerp(from.c, to.c, t);
925 d.lerp(from.d, to.d, t);
926 }
927
identitymatrix4x3928 void identity()
929 {
930 a = vec(1, 0, 0);
931 b = vec(0, 1, 0);
932 c = vec(0, 0, 1);
933 d = vec(0, 0, 0);
934 }
935
mulmatrix4x3936 void mul(const matrix4x3 &m, const matrix4x3 &n)
937 {
938 a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
939 b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
940 c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
941 d = vec(m.d).madd(m.a, n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
942 }
mulmatrix4x3943 void mul(const matrix4x3 &n) { mul(matrix4x3(*this), n); }
944
mulmatrix4x3945 void mul(const matrix3 &m, const matrix4x3 &n)
946 {
947 a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
948 b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
949 c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
950 d = vec(m.a).mul(n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
951 }
952
mulmatrix4x3953 void mul(const matrix3 &rot, const vec &trans, const matrix4x3 &n)
954 {
955 mul(rot, n);
956 d.add(trans);
957 }
958
transposematrix4x3959 void transpose()
960 {
961 d = vec(a.dot(d), b.dot(d), c.dot(d)).neg();
962 swap(a.y, b.x); swap(a.z, c.x);
963 swap(b.z, c.y);
964 }
965
transposematrix4x3966 void transpose(const matrix4x3 &o)
967 {
968 a = vec(o.a.x, o.b.x, o.c.x);
969 b = vec(o.a.y, o.b.y, o.c.y);
970 c = vec(o.a.z, o.b.z, o.c.z);
971 d = vec(o.a.dot(o.d), o.b.dot(o.d), o.c.dot(o.d)).neg();
972 }
973
transposemulmatrix4x3974 void transposemul(const matrix4x3 &m, const matrix4x3 &n)
975 {
976 vec t(m.a.dot(m.d), m.b.dot(m.d), m.c.dot(m.d));
977 a = vec(m.a.dot(n.a), m.b.dot(n.a), m.c.dot(n.a));
978 b = vec(m.a.dot(n.b), m.b.dot(n.b), m.c.dot(n.b));
979 c = vec(m.a.dot(n.c), m.b.dot(n.c), m.c.dot(n.c));
980 d = vec(m.a.dot(n.d), m.b.dot(n.d), m.c.dot(n.d)).sub(t);
981 }
982
multransposematrix4x3983 void multranspose(const matrix4x3 &m, const matrix4x3 &n)
984 {
985 vec t(n.a.dot(n.d), n.b.dot(n.d), n.c.dot(n.d));
986 a = vec(m.a).mul(n.a.x).madd(m.b, n.b.x).madd(m.c, n.c.x);
987 b = vec(m.a).mul(n.a.y).madd(m.b, m.b.y).madd(m.c, n.c.y);
988 c = vec(m.a).mul(n.a.z).madd(m.b, n.b.z).madd(m.c, n.c.z);
989 d = vec(m.d).msub(m.a, t.x).msub(m.b, t.y).msub(m.c, t.z);
990 }
991
invertmatrix4x3992 void invert(const matrix4x3 &o)
993 {
994 vec unscale(1/o.a.squaredlen(), 1/o.b.squaredlen(), 1/o.c.squaredlen());
995 transpose(o);
996 a.mul(unscale);
997 b.mul(unscale);
998 c.mul(unscale);
999 d.mul(unscale);
1000 }
invertmatrix4x31001 void invert() { invert(matrix4x3(*this)); }
1002
rotatematrix4x31003 void rotate(float angle, const vec &d)
1004 {
1005 rotate(cosf(angle), sinf(angle), d);
1006 }
1007
rotatematrix4x31008 void rotate(float ck, float sk, const vec &axis)
1009 {
1010 matrix3 m;
1011 m.rotate(ck, sk, axis);
1012 *this = matrix4x3(m, vec(0, 0, 0));
1013 }
1014
rotate_around_xmatrix4x31015 void rotate_around_x(float ck, float sk)
1016 {
1017 vec rb = vec(b).mul(ck).madd(c, sk),
1018 rc = vec(c).mul(ck).msub(b, sk);
1019 b = rb;
1020 c = rc;
1021 }
rotate_around_xmatrix4x31022 void rotate_around_x(float angle) { rotate_around_x(cosf(angle), sinf(angle)); }
rotate_around_xmatrix4x31023 void rotate_around_x(const vec2 &sc) { rotate_around_x(sc.x, sc.y); }
1024
rotate_around_ymatrix4x31025 void rotate_around_y(float ck, float sk)
1026 {
1027 vec rc = vec(c).mul(ck).madd(a, sk),
1028 ra = vec(a).mul(ck).msub(c, sk);
1029 c = rc;
1030 a = ra;
1031 }
rotate_around_ymatrix4x31032 void rotate_around_y(float angle) { rotate_around_y(cosf(angle), sinf(angle)); }
rotate_around_ymatrix4x31033 void rotate_around_y(const vec2 &sc) { rotate_around_y(sc.x, sc.y); }
1034
rotate_around_zmatrix4x31035 void rotate_around_z(float ck, float sk)
1036 {
1037 vec ra = vec(a).mul(ck).madd(b, sk),
1038 rb = vec(b).mul(ck).msub(a, sk);
1039 a = ra;
1040 b = rb;
1041 }
rotate_around_zmatrix4x31042 void rotate_around_z(float angle) { rotate_around_z(cosf(angle), sinf(angle)); }
rotate_around_zmatrix4x31043 void rotate_around_z(const vec2 &sc) { rotate_around_z(sc.x, sc.y); }
1044
transformmatrix4x31045 vec transform(const vec &o) const { return vec(d).madd(a, o.x).madd(b, o.y).madd(c, o.z); }
transposedtransformmatrix4x31046 vec transposedtransform(const vec &o) const { vec p = vec(o).sub(d); return vec(a.dot(p), b.dot(p), c.dot(p)); }
transformnormalmatrix4x31047 vec transformnormal(const vec &o) const { return vec(a).mul(o.x).madd(b, o.y).madd(c, o.z); }
transposedtransformnormalmatrix4x31048 vec transposedtransformnormal(const vec &o) const { return vec(a.dot(o), b.dot(o), c.dot(o)); }
transformmatrix4x31049 vec transform(const vec2 &o) const { return vec(d).madd(a, o.x).madd(b, o.y); }
1050
rowxmatrix4x31051 vec4 rowx() const { return vec4(a.x, b.x, c.x, d.x); }
rowymatrix4x31052 vec4 rowy() const { return vec4(a.y, b.y, c.y, d.y); }
rowzmatrix4x31053 vec4 rowz() const { return vec4(a.z, b.z, c.z, d.z); }
1054 };
1055
dualquat(const matrix4x3 & m)1056 inline dualquat::dualquat(const matrix4x3 &m) : real(m)
1057 {
1058 dual.x = 0.5f*( m.d.x*real.w + m.d.y*real.z - m.d.z*real.y);
1059 dual.y = 0.5f*(-m.d.x*real.z + m.d.y*real.w + m.d.z*real.x);
1060 dual.z = 0.5f*( m.d.x*real.y - m.d.y*real.x + m.d.z*real.w);
1061 dual.w = -0.5f*( m.d.x*real.x + m.d.y*real.y + m.d.z*real.z);
1062 }
1063
matrix3(const matrix4x3 & m)1064 inline matrix3::matrix3(const matrix4x3 &m) : a(m.a), b(m.b), c(m.c) {}
1065
1066 struct plane : vec
1067 {
1068 float offset;
1069
distplane1070 float dist(const vec &p) const { return dot(p)+offset; }
distplane1071 float dist(const vec4 &p) const { return p.dot3(*this) + p.w*offset; }
1072 bool operator==(const plane &p) const { return x==p.x && y==p.y && z==p.z && offset==p.offset; }
1073 bool operator!=(const plane &p) const { return x!=p.x || y!=p.y || z!=p.z || offset!=p.offset; }
1074
planeplane1075 plane() {}
planeplane1076 plane(const vec &c, float off) : vec(c), offset(off) {}
planeplane1077 plane(const vec4 &p) : vec(p), offset(p.w) {}
planeplane1078 plane(int d, float off)
1079 {
1080 x = y = z = 0.0f;
1081 v[d] = 1.0f;
1082 offset = -off;
1083 }
planeplane1084 plane(float a, float b, float c, float d) : vec(a, b, c), offset(d) {}
1085
toplaneplane1086 void toplane(const vec &n, const vec &p)
1087 {
1088 x = n.x; y = n.y; z = n.z;
1089 offset = -dot(p);
1090 }
1091
toplaneplane1092 bool toplane(const vec &a, const vec &b, const vec &c)
1093 {
1094 cross(vec(b).sub(a), vec(c).sub(a));
1095 float mag = magnitude();
1096 if(!mag) return false;
1097 div(mag);
1098 offset = -dot(a);
1099 return true;
1100 }
1101
rayintersectplane1102 bool rayintersect(const vec &o, const vec &ray, float &dist)
1103 {
1104 float cosalpha = dot(ray);
1105 if(cosalpha==0) return false;
1106 float deltac = offset+dot(o);
1107 dist -= deltac/cosalpha;
1108 return true;
1109 }
1110
reflectzplane1111 plane &reflectz(float rz)
1112 {
1113 offset += 2*rz*z;
1114 z = -z;
1115 return *this;
1116 }
1117
invertplane1118 plane &invert()
1119 {
1120 neg();
1121 offset = -offset;
1122 return *this;
1123 }
1124
scaleplane1125 plane &scale(float k)
1126 {
1127 mul(k);
1128 return *this;
1129 }
1130
translateplane1131 plane &translate(const vec &p)
1132 {
1133 offset += dot(p);
1134 return *this;
1135 }
1136
normalizeplane1137 plane &normalize()
1138 {
1139 float mag = magnitude();
1140 div(mag);
1141 offset /= mag;
1142 return *this;
1143 }
1144
zintersectplane1145 float zintersect(const vec &p) const { return -(x*p.x+y*p.y+offset)/z; }
zdeltaplane1146 float zdelta(const vec &p) const { return -(x*p.x+y*p.y)/z; }
zdistplane1147 float zdist(const vec &p) const { return p.z-zintersect(p); }
1148 };
1149
1150 struct triangle
1151 {
1152 vec a, b, c;
1153
triangletriangle1154 triangle(const vec &a, const vec &b, const vec &c) : a(a), b(b), c(c) {}
triangletriangle1155 triangle() {}
1156
addtriangle1157 triangle &add(const vec &o) { a.add(o); b.add(o); c.add(o); return *this; }
subtriangle1158 triangle &sub(const vec &o) { a.sub(o); b.sub(o); c.sub(o); return *this; }
1159
1160 bool operator==(const triangle &t) const { return a == t.a && b == t.b && c == t.c; }
1161 };
1162
1163 /**
1164
1165 The engine uses 3 different linear coordinate systems
1166 which are oriented around each of the axis dimensions.
1167
1168 So any point within the game can be defined by four coordinates: (d, x, y, z)
1169
1170 d is the reference axis dimension
1171 x is the coordinate of the ROW dimension
1172 y is the coordinate of the COL dimension
1173 z is the coordinate of the reference dimension (DEPTH)
1174
1175 typically, if d is not used, then it is implicitly the Z dimension.
1176 ie: d=z => x=x, y=y, z=z
1177
1178 **/
1179
1180 // DIM: X=0 Y=1 Z=2.
1181 const int R[3] = {1, 2, 0}; // row
1182 const int C[3] = {2, 0, 1}; // col
1183 const int D[3] = {0, 1, 2}; // depth
1184
1185 struct ivec4;
1186 struct ivec2;
1187
1188 struct ivec
1189 {
1190 union
1191 {
1192 struct { int x, y, z; };
1193 struct { int r, g, b; };
1194 int v[3];
1195 };
1196
ivecivec1197 ivec() {}
ivecivec1198 ivec(const vec &v) : x(int(v.x)), y(int(v.y)), z(int(v.z)) {}
ivecivec1199 ivec(int a, int b, int c) : x(a), y(b), z(c) {}
ivecivec1200 ivec(int d, int row, int col, int depth)
1201 {
1202 v[R[d]] = row;
1203 v[C[d]] = col;
1204 v[D[d]] = depth;
1205 }
ivecivec1206 ivec(int i, const ivec &co, int size) : x(co.x+((i&1)>>0)*size), y(co.y+((i&2)>>1)*size), z(co.z +((i&4)>>2)*size) {}
1207 explicit ivec(const ivec4 &v);
1208 explicit ivec(const ivec2 &v, int z = 0);
1209 explicit ivec(const usvec &v);
1210 explicit ivec(const svec &v);
1211
1212 int &operator[](int i) { return v[i]; }
1213 int operator[](int i) const { return v[i]; }
1214
1215 //int idx(int i) { return v[i]; }
1216 bool operator==(const ivec &v) const { return x==v.x && y==v.y && z==v.z; }
1217 bool operator!=(const ivec &v) const { return x!=v.x || y!=v.y || z!=v.z; }
iszeroivec1218 bool iszero() const { return x==0 && y==0 && z==0; }
shlivec1219 ivec &shl(int n) { x<<= n; y<<= n; z<<= n; return *this; }
shrivec1220 ivec &shr(int n) { x>>= n; y>>= n; z>>= n; return *this; }
mulivec1221 ivec &mul(int n) { x *= n; y *= n; z *= n; return *this; }
divivec1222 ivec &div(int n) { x /= n; y /= n; z /= n; return *this; }
addivec1223 ivec &add(int n) { x += n; y += n; z += n; return *this; }
subivec1224 ivec &sub(int n) { x -= n; y -= n; z -= n; return *this; }
mulivec1225 ivec &mul(const ivec &v) { x *= v.x; y *= v.y; z *= v.z; return *this; }
divivec1226 ivec &div(const ivec &v) { x /= v.x; y /= v.y; z /= v.z; return *this; }
addivec1227 ivec &add(const ivec &v) { x += v.x; y += v.y; z += v.z; return *this; }
subivec1228 ivec &sub(const ivec &v) { x -= v.x; y -= v.y; z -= v.z; return *this; }
maskivec1229 ivec &mask(int n) { x &= n; y &= n; z &= n; return *this; }
negivec1230 ivec &neg() { x = -x; y = -y; z = -z; return *this; }
minivec1231 ivec &min(const ivec &o) { x = ::min(x, o.x); y = ::min(y, o.y); z = ::min(z, o.z); return *this; }
maxivec1232 ivec &max(const ivec &o) { x = ::max(x, o.x); y = ::max(y, o.y); z = ::max(z, o.z); return *this; }
minivec1233 ivec &min(int n) { x = ::min(x, n); y = ::min(y, n); z = ::min(z, n); return *this; }
maxivec1234 ivec &max(int n) { x = ::max(x, n); y = ::max(y, n); z = ::max(z, n); return *this; }
absivec1235 ivec &abs() { x = ::abs(x); y = ::abs(y); z = ::abs(z); return *this; }
clampivec1236 ivec &clamp(int l, int h) { x = ::clamp(x, l, h); y = ::clamp(y, l, h); z = ::clamp(z, l, h); return *this; }
crossivec1237 ivec &cross(const ivec &a, const ivec &b) { x = a.y*b.z-a.z*b.y; y = a.z*b.x-a.x*b.z; z = a.x*b.y-a.y*b.x; return *this; }
dotivec1238 int dot(const ivec &o) const { return x*o.x + y*o.y + z*o.z; }
distivec1239 float dist(const plane &p) const { return x*p.x + y*p.y + z*p.z + p.offset; }
1240
floorivec1241 static inline ivec floor(const vec &o) { return ivec(int(::floor(o.x)), int(::floor(o.y)), int(::floor(o.z))); }
ceilivec1242 static inline ivec ceil(const vec &o) { return ivec(int(::ceil(o.x)), int(::ceil(o.y)), int(::ceil(o.z))); }
1243 };
1244
vec(const ivec & v)1245 inline vec::vec(const ivec &v) : x(v.x), y(v.y), z(v.z) {}
1246
htcmp(const ivec & x,const ivec & y)1247 static inline bool htcmp(const ivec &x, const ivec &y)
1248 {
1249 return x == y;
1250 }
1251
hthash(const ivec & k)1252 static inline uint hthash(const ivec &k)
1253 {
1254 return k.x^k.y^k.z;
1255 }
1256
1257 struct ivec2
1258 {
1259 union
1260 {
1261 struct { int x, y; };
1262 int v[2];
1263 };
1264
ivec2ivec21265 ivec2() {}
ivec2ivec21266 ivec2(int x, int y) : x(x), y(y) {}
ivec2ivec21267 explicit ivec2(const vec2 &v) : x(int(v.x)), y(int(v.y)) {}
ivec2ivec21268 explicit ivec2(const ivec &v) : x(v.x), y(v.y) {}
1269
1270 int &operator[](int i) { return v[i]; }
1271 int operator[](int i) const { return v[i]; }
1272
1273 bool operator==(const ivec2 &o) const { return x == o.x && y == o.y; }
1274 bool operator!=(const ivec2 &o) const { return x != o.x || y != o.y; }
1275
iszeroivec21276 bool iszero() const { return x==0 && y==0; }
shlivec21277 ivec2 &shl(int n) { x<<= n; y<<= n; return *this; }
shrivec21278 ivec2 &shr(int n) { x>>= n; y>>= n; return *this; }
mulivec21279 ivec2 &mul(int n) { x *= n; y *= n; return *this; }
divivec21280 ivec2 &div(int n) { x /= n; y /= n; return *this; }
addivec21281 ivec2 &add(int n) { x += n; y += n; return *this; }
subivec21282 ivec2 &sub(int n) { x -= n; y -= n; return *this; }
mulivec21283 ivec2 &mul(const ivec2 &v) { x *= v.x; y *= v.y; return *this; }
divivec21284 ivec2 &div(const ivec2 &v) { x /= v.x; y /= v.y; return *this; }
addivec21285 ivec2 &add(const ivec2 &v) { x += v.x; y += v.y; return *this; }
subivec21286 ivec2 &sub(const ivec2 &v) { x -= v.x; y -= v.y; return *this; }
maskivec21287 ivec2 &mask(int n) { x &= n; y &= n; return *this; }
negivec21288 ivec2 &neg() { x = -x; y = -y; return *this; }
minivec21289