forked from gildor2/UEViewer
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Math3D.h
316 lines (268 loc) · 8.07 KB
/
Math3D.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
#ifndef __MATH_3D_H__
#define __MATH_3D_H__
inline void FNegate(const float &a, float &b)
{
b = -a;
}
inline void FNegate(float &a)
{
a = -a;
}
inline bool IsNegative(float f)
{
return f < 0;
}
inline float appRsqrt(float number)
{
return 1.0f / sqrt(number);
}
template<class T> inline T Lerp(const T& A, const T& B, float Alpha)
{
return A + Alpha * (B-A);
}
/*-----------------------------------------------------------------------------
Vector
-----------------------------------------------------------------------------*/
struct CVec3
{
union
{
float v[3];
struct { float X, Y, Z; };
};
// access to data
inline float& operator[](int index)
{
return v[index];
}
inline const float& operator[](int index) const
{
return v[index];
}
// NOTE: for those functions which requires CVec3 -> float*, we can easily do it using CVec3.v field
// trivial setup functions
inline void Zero()
{
memset(this, 0, sizeof(CVec3));
}
inline void Set(float x, float y, float z)
{
v[0] = x; v[1] = y; v[2] = z;
}
// simple math
inline void Negate()
{
FNegate(v[0]);
FNegate(v[1]);
FNegate(v[2]);
}
//!! +NegateTo(dst)
inline void Scale(float scale)
{
v[0] *= scale;
v[1] *= scale;
v[2] *= scale;
}
inline void Scale(const CVec3 &scale)
{
v[0] *= scale[0];
v[1] *= scale[1];
v[2] *= scale[2];
}
inline void Add(const CVec3 &a) //?? == "operator +=(CVec3&)"
{
v[0] += a.v[0];
v[1] += a.v[1];
v[2] += a.v[2];
}
inline void Sub(const CVec3 &a) //?? == "operator -=(CVec3&)"
{
v[0] -= a.v[0];
v[1] -= a.v[1];
v[2] -= a.v[2];
}
//!! +ScaleTo(dst)
float GetLength() const;
inline float GetLengthSq() const
{
return dot(*this, *this);
}
float Normalize(); // returns vector length
float NormalizeFast(); //?? 2-arg version too?
void FindAxisVectors(CVec3 &right, CVec3 &up) const;
friend float dot(const CVec3&, const CVec3&);
};
inline bool operator==(const CVec3 &v1, const CVec3 &v2)
{
return memcmp(&v1, &v2, sizeof(CVec3)) == 0;
}
inline bool operator!=(const CVec3 &v1, const CVec3 &v2)
{
return memcmp(&v1, &v2, sizeof(CVec3)) != 0;
}
inline void Lerp(const CVec3 &A, const CVec3 &B, float Alpha, CVec3 &dst)
{
dst[0] = A.v[0] + Alpha * (B.v[0]-A.v[0]);
dst[1] = A.v[1] + Alpha * (B.v[1]-A.v[1]);
dst[2] = A.v[2] + Alpha * (B.v[2]-A.v[2]);
}
inline float dot(const CVec3 &a, const CVec3 &b)
{
// return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
return a.v[0]*b.v[0] + a.v[1]*b.v[1] + a.v[2]*b.v[2];
}
void cross(const CVec3 &v1, const CVec3 &v2, CVec3 &result);
//?? OLD-STYLE FUNCTIONS :
inline void VectorSubtract(const CVec3 &a, const CVec3 &b, CVec3 &d)
{
d.v[0] = a.v[0] - b.v[0];
d.v[1] = a.v[1] - b.v[1];
d.v[2] = a.v[2] - b.v[2];
}
inline void VectorAdd(const CVec3 &a, const CVec3 &b, CVec3 &d)
{
d.v[0] = a.v[0] + b.v[0];
d.v[1] = a.v[1] + b.v[1];
d.v[2] = a.v[2] + b.v[2];
}
inline void VectorNegate(const CVec3 &a, CVec3 &b)
{
FNegate(a.v[0], b.v[0]);
FNegate(a.v[1], b.v[1]);
FNegate(a.v[2], b.v[2]);
}
inline void VectorScale(const CVec3 &a, float scale, CVec3 &b)
{
b.v[0] = scale * a.v[0];
b.v[1] = scale * a.v[1];
b.v[2] = scale * a.v[2];
}
// d = a + scale * b
inline void VectorMA(const CVec3 &a, float scale, const CVec3 &b, CVec3 &d)
{
d.v[0] = a.v[0] + scale * b.v[0];
d.v[1] = a.v[1] + scale * b.v[1];
d.v[2] = a.v[2] + scale * b.v[2];
}
// a += scale * b
inline void VectorMA(CVec3 &a, float scale, const CVec3 &b) //!! method, rename?
{
a.v[0] += scale * b.v[0];
a.v[1] += scale * b.v[1];
a.v[2] += scale * b.v[2];
}
float VectorDistance(const CVec3 &vec1, const CVec3 &vec2);
//?? VectorDistanceFast()
float VectorNormalize(const CVec3 &v, CVec3 &out);
/*-----------------------------------------------------------------------------
Axis
-----------------------------------------------------------------------------*/
struct CAxis
{
// fields
// NOTE: v[] can be private, but this will prevent from
// initializing CAxis object with initializer list ( "= {n,n,n,n ...}" )
CVec3 v[3];
// methods
void FromEuler(const CVec3 &angles);
void TransformVector(const CVec3 &src, CVec3 &dst) const; // orthonormal 'this'
void TransformVectorSlow(const CVec3 &src, CVec3 &dst) const; // any 'this'
void UnTransformVector(const CVec3 &src, CVec3 &dst) const;
void TransformAxis(const CAxis &src, CAxis &dst) const; // orthonormal 'this'
void TransformAxisSlow(const CAxis &src, CAxis &dst) const; // any 'this'
void UnTransformAxis(const CAxis &src, CAxis &dst) const;
void PrescaleSource(const CVec3 &scale); // scale vector before transformation by this axis
// indexed access
inline CVec3& operator[](int index)
{
return v[index];
}
inline const CVec3& operator[](int index) const
{
return v[index];
}
};
struct CCoords
{
// fields
CVec3 origin;
CAxis axis;
// methods
void Scale(float scale)
{
float *f = (float*)this;
for (int i = 0; i < 12; i++, f++)
*f *= scale;
}
void TransformPoint(const CVec3 &src, CVec3 &dst) const; // orthonormal 'this'
void TransformPointSlow(const CVec3 &src, CVec3 &dst) const; // any 'this'
void UnTransformPoint(const CVec3 &src, CVec3 &dst) const;
void TransformCoords(const CCoords &src, CCoords &dst) const; // orthonormal 'this'
void TransformCoordsSlow(const CCoords &src, CCoords &dst) const; // any 'this'
void UnTransformCoords(const CCoords &src, CCoords &dst) const;
};
// Functions for work with coordinate systems, not combined into CCoords class
// global coordinate system -> local coordinate system (src -> dst) by origin/axis coords
void TransformPoint(const CVec3 &origin, const CAxis &axis, const CVec3 &src, CVec3 &dst);
// local coordinate system -> global coordinate system
void UnTransformPoint(const CVec3 &origin, const CAxis &axis, const CVec3 &src, CVec3 &dst);
// compute reverse transformation
void InvertCoords(const CCoords &S, CCoords &D); // orthonormal S
void InvertCoordsSlow(const CCoords &S, CCoords &D); // any S
// a += scale * b
void CoordsMA(CCoords &a, float scale, const CCoords &b);
/*-----------------------------------------------------------------------------
Some constants
-----------------------------------------------------------------------------*/
#define nullVec3 identCoords.origin
//extern const CBox nullBox;
#define identAxis identCoords.axis
extern const CCoords identCoords;
/*-----------------------------------------------------------------------------
Quaternion
-----------------------------------------------------------------------------*/
struct CQuat
{
float X, Y, Z, W;
inline void Set(float _x, float _y, float _z, float _w)
{
X = _x; Y = _y; Z = _z; W = _w;
}
void FromAxis(const CAxis &src);
void ToAxis(CAxis &dst) const;
float GetLength() const;
// Build a unit quaternion representing the rotation from v1 to v2. The input vectors need not be normalized.
void FromTwoVectors(const CVec3& v1, const CVec3& v2);
void RotateVector(const CVec3& src, CVec3& dst) const;
inline void Conjugate()
{
FNegate(X);
FNegate(Y);
FNegate(Z);
}
inline void Negate()
{
FNegate(X);
FNegate(Y);
FNegate(Z);
FNegate(W);
}
void Normalize();
void Mul(const CQuat &Q);
};
void Slerp(const CQuat &A, const CQuat &B, float Alpha, CQuat &dst);
/*-----------------------------------------------------------------------------
Angle math
-----------------------------------------------------------------------------*/
// angle indexes
enum
{
PITCH, // looking up and down (0=Straight Ahead, +Up, -Down).
YAW, // rotating around (running in circles), 0=East, +North, -South.
ROLL // rotation about axis of screen, 0=Straight, +Clockwise, -CCW.
};
void Euler2Vecs(const CVec3 &angles, CVec3 *forward, CVec3 *right, CVec3 *up);
void Vec2Euler(const CVec3 &vec, CVec3 &angles);
float Vec2Yaw(const CVec3 &vec);
void Quat2Euler(const CQuat& quat, CVec3& angles);
#endif // __MATH_3D_H__