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
|
/////////////////////////////////////////////////////////////////////////////
// Name: mathstuff.cpp
// Purpose: Some maths used for pyramid sample
// Author: Manuel Martin
// Created: 2015/01/31
// Copyright: (c) 2015 Manuel Martin
// Licence: wxWindows licence
/////////////////////////////////////////////////////////////////////////////
#include <cmath>
#include "mathstuff.h"
// Overload of "-" operator
myVec3 operator- (const myVec3& v1, const myVec3& v2)
{
return myVec3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}
// Vector normalization
myVec3 MyNormalize(const myVec3& v)
{
double mo = sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
if ( mo > 1E-20 )
return myVec3(v.x / mo, v.y / mo, v.z / mo);
else
return myVec3();
}
// Dot product
double MyDot(const myVec3& v1, const myVec3& v2)
{
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z ;
}
// Cross product
myVec3 MyCross(const myVec3& v1, const myVec3& v2)
{
return myVec3( v1.y * v2.z - v2.y * v1.z,
v1.z * v2.x - v2.z * v1.x,
v1.x * v2.y - v2.x * v1.y );
}
// Distance between two points
double MyDistance(const myVec3& v1, const myVec3& v2)
{
double rx = v1.x -v2.x;
double ry = v1.y -v2.y;
double rz = v1.z -v2.z;
return sqrt(rx*rx + ry*ry + rz*rz);
}
// Angle between two normalized vectors, in radians
double AngleBetween(const myVec3& v1, const myVec3& v2)
{
double angle = MyDot(v1, v2);
// Prevent issues due to numerical precision
if (angle > 1.0)
angle = 1.0;
if (angle < -1.0)
angle = -1.0;
return acos(angle);
}
// Matrix 4x4 by 4x1 multiplication
// Attention: No bounds check!
myVec4 MyMatMul4x1(const double *m1, const myVec4& v)
{
myVec4 mmv;
mmv.x = m1[0] * v.x + m1[4] * v.y + m1[8] * v.z + m1[12] * v.w ;
mmv.y = m1[1] * v.x + m1[5] * v.y + m1[9] * v.z + m1[13] * v.w ;
mmv.z = m1[2] * v.x + m1[6] * v.y + m1[10] * v.z + m1[14] * v.w ;
mmv.w = m1[3] * v.x + m1[7] * v.y + m1[11] * v.z + m1[15] * v.w ;
return mmv;
}
// Matrix 4x4 multiplication
// Attention: No bounds check!
void MyMatMul4x4(const double *m1, const double *m2, double* mm)
{
mm[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3] ;
mm[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3] ;
mm[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3] ;
mm[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3] ;
mm[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7] ;
mm[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7] ;
mm[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7] ;
mm[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7] ;
mm[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11] ;
mm[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11] ;
mm[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11] ;
mm[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11] ;
mm[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15] ;
mm[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15] ;
mm[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15] ;
mm[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15] ;
}
// Matrix 4x4 inverse. Returns the determinant.
// Attention: No bounds check!
// Method used is "adjugate matrix" with "cofactors".
// A faster method, such as "LU decomposition", isn't much faster than this code.
double MyMatInverse(const double *m, double *minv)
{
double det;
double cof[16], sdt[19];
// The 2x2 determinants used for cofactors
sdt[0] = m[10] * m[15] - m[14] * m[11] ;
sdt[1] = m[9] * m[15] - m[13] * m[11] ;
sdt[2] = m[9] * m[14] - m[13] * m[10] ;
sdt[3] = m[8] * m[15] - m[12] * m[11] ;
sdt[4] = m[8] * m[14] - m[12] * m[10] ;
sdt[5] = m[8] * m[13] - m[12] * m[9] ;
sdt[6] = m[6] * m[15] - m[14] * m[7] ;
sdt[7] = m[5] * m[15] - m[13] * m[7] ;
sdt[8] = m[5] * m[14] - m[13] * m[6] ;
sdt[9] = m[4] * m[15] - m[12] * m[7] ;
sdt[10] = m[4] * m[14] - m[12] * m[6] ;
sdt[11] = m[5] * m[15] - m[13] * m[7] ;
sdt[12] = m[4] * m[13] - m[12] * m[5] ;
sdt[13] = m[6] * m[11] - m[10] * m[7] ;
sdt[14] = m[5] * m[11] - m[9] * m[7] ;
sdt[15] = m[5] * m[10] - m[9] * m[6] ;
sdt[16] = m[4] * m[11] - m[8] * m[7] ;
sdt[17] = m[4] * m[10] - m[8] * m[6] ;
sdt[18] = m[4] * m[9] - m[8] * m[5] ;
// The cofactors, transposed
cof[0] = m[5] * sdt[0] - m[6] * sdt[1] + m[7] * sdt[2] ;
cof[1] = - m[1] * sdt[0] + m[2] * sdt[1] - m[3] * sdt[2] ;
cof[2] = m[1] * sdt[6] - m[2] * sdt[7] + m[3] * sdt[8] ;
cof[3] = - m[1] * sdt[13] + m[2] * sdt[14] - m[3] * sdt[15] ;
cof[4] = - m[4] * sdt[0] + m[6] * sdt[3] - m[7] * sdt[4] ;
cof[5] = m[0] * sdt[0] - m[2] * sdt[3] + m[3] * sdt[4] ;
cof[6] = - m[0] * sdt[6] + m[2] * sdt[9] - m[3] * sdt[10] ;
cof[7] = m[0] * sdt[13] - m[2] * sdt[16] + m[3] * sdt[17] ;
cof[8] = m[4] * sdt[1] - m[5] * sdt[3] + m[7] * sdt[5] ;
cof[9] = - m[0] * sdt[1] + m[1] * sdt[3] - m[3] * sdt[5] ;
cof[10] = m[0] * sdt[11] - m[1] * sdt[9] + m[3] * sdt[12] ;
cof[11] = - m[0] * sdt[14] + m[1] * sdt[16] - m[3] * sdt[18] ;
cof[12] = - m[4] * sdt[2] + m[5] * sdt[4] - m[6] * sdt[5] ;
cof[13] = m[0] * sdt[2] - m[1] * sdt[4] + m[2] * sdt[5] ;
cof[14] = - m[0] * sdt[8] + m[1] * sdt[10] - m[2] * sdt[12] ;
cof[15] = m[0] * sdt[15] - m[1] * sdt[17] + m[2] * sdt[18] ;
det = m[0] * cof[0] + m[1] * cof[4] + m[2] * cof[8] + m[3] * cof[12] ;
if ( fabs(det) > 10E-9 ) // Some precision value
{
double invdet = 1.0 / det;
for (int i = 0; i < 16; ++i)
minv[i] = cof[i] * invdet;
}
else
{
// Enable comparison with 0
det = 0.0;
}
return det;
}
// Matrix of rotation around an axis in the origin.
// angle is positive if follows axis (right-hand rule)
// Attention: No bounds check!
void MyRotate(const myVec3& axis, double angle, double *mrot)
{
double c = cos(angle);
double s = sin(angle);
double t = 1.0 - c;
// Normalize the axis vector
myVec3 uv = MyNormalize(axis);
// Store the matrix in column order
mrot[0] = t * uv.x * uv.x + c ;
mrot[1] = t * uv.x * uv.y + s * uv.z ;
mrot[2] = t * uv.x * uv.z - s * uv.y ;
mrot[3] = 0.0 ;
mrot[4] = t * uv.y * uv.x - s * uv.z ;
mrot[5] = t * uv.y * uv.y + c ;
mrot[6] = t * uv.y * uv.z + s * uv.x ;
mrot[7] = 0.0 ;
mrot[8] = t * uv.z * uv.x + s * uv.y ;
mrot[9] = t * uv.z * uv.y - s * uv.x ;
mrot[10] = t * uv.z * uv.z + c ;
mrot[11] = 0.0 ;
mrot[12] = mrot[13] = mrot[14] = 0.0 ;
mrot[15] = 1.0 ;
}
// Matrix for defining the viewing transformation
// Attention: No bounds check!
// Unchecked conditions:
// camPos != targ && camUp != {0,0,0}
// camUo can't be parallel to camPos - targ
void MyLookAt(const myVec3& camPos, const myVec3& camUp, const myVec3& targ, double *mt)
{
myVec3 tc = MyNormalize(targ - camPos);
myVec3 up = MyNormalize(camUp);
// Normalize tc x up for the case where up is not perpendicular to tc
myVec3 s = MyNormalize(MyCross(tc, up));
myVec3 u = MyNormalize(MyCross(s, tc)); //Normalize to improve accuracy
// Store the matrix in column order
mt[0] = s.x ;
mt[1] = u.x ;
mt[2] = - tc.x ;
mt[3] = 0.0 ;
mt[4] = s.y ;
mt[5] = u.y ;
mt[6] = - tc.y ;
mt[7] = 0.0 ;
mt[8] = s.z ;
mt[9] = u.z ;
mt[10] = - tc.z ;
mt[11] = 0.0 ;
mt[12] = - MyDot(s, camPos) ;
mt[13] = - MyDot(u, camPos) ;
mt[14] = MyDot(tc, camPos) ;
mt[15] = 1.0 ;
}
// Matrix for defining the perspective projection with symmetric frustum
// From camera coordinates to canonical (2x2x2 cube) coordinates.
// Attention: No bounds check!
// Unchecked conditions: fov > 0 && zNear > 0 && zFar > zNear && aspect > 0
void MyPerspective(double fov, double aspect, double zNear, double zFar, double *mp)
{
double f = 1.0 / tan(fov / 2.0);
// Store the matrix in column order
mp[0] = f / aspect ;
mp[1] = mp[2] = mp[3] = 0.0 ;
mp[4] = 0.0 ;
mp[5] = f ;
mp[6] = mp[7] = 0.0 ;
mp[8] = mp[9] = 0.0 ;
mp[10] = (zNear + zFar) / (zNear - zFar) ;
mp[11] = -1.0 ;
mp[12] = mp[13] = 0.0 ;
mp[14] = 2.0 * zNear * zFar / (zNear - zFar) ;
mp[15] = 0.0 ;
}
// Matrix for defining the orthogonal projection with symmetric frustum
// From camera coordinates to canonical (2x2x2 cube) coordinates.
// Attention: No bounds check!
// Unchecked conditions: left != right && bottom != top && zNear != zFar
void MyOrtho(double left, double right, double bottom, double top,
double zNear, double zFar, double *mo)
{
// Store the matrix in column order
mo[0] = 2.0 / (right - left) ;
mo[1] = mo[2] = mo[3] = mo[4] = 0.0 ;
mo[5] = 2.0 / (top - bottom) ;
mo[6] = mo[7] = mo[8] = mo[9] = 0.0 ;
mo[10] = 2.0 / (zNear - zFar) ;
mo[11] = 0.0 ;
mo[12] = -(right + left) / (right - left) ;
mo[13] = -(top + bottom) / (top - bottom) ;
mo[14] = (zNear + zFar) / (zNear - zFar) ;
mo[15] = 1.0 ;
}
|
