1 /////////////////////////////////////////////////////////////////////////////
2 // Name: mathstuff.cpp
3 // Purpose: Some maths used for pyramid sample
4 // Author: Manuel Martin
5 // Created: 2015/01/31
6 // Copyright: (c) 2015 Manuel Martin
7 // Licence: wxWindows licence
8 /////////////////////////////////////////////////////////////////////////////
9
10 #include <cmath>
11
12 #include "mathstuff.h"
13
14 // Overload of "-" operator
operator -(const myVec3 & v1,const myVec3 & v2)15 myVec3 operator- (const myVec3& v1, const myVec3& v2)
16 {
17 return myVec3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
18 }
19
20 // Vector normalization
MyNormalize(const myVec3 & v)21 myVec3 MyNormalize(const myVec3& v)
22 {
23 double mo = sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
24 if ( mo > 1E-20 )
25 return myVec3(v.x / mo, v.y / mo, v.z / mo);
26 else
27 return myVec3();
28 }
29
30 // Dot product
MyDot(const myVec3 & v1,const myVec3 & v2)31 double MyDot(const myVec3& v1, const myVec3& v2)
32 {
33 return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z ;
34 }
35
36 // Cross product
MyCross(const myVec3 & v1,const myVec3 & v2)37 myVec3 MyCross(const myVec3& v1, const myVec3& v2)
38 {
39 return myVec3( v1.y * v2.z - v2.y * v1.z,
40 v1.z * v2.x - v2.z * v1.x,
41 v1.x * v2.y - v2.x * v1.y );
42 }
43
44 // Distance between two points
MyDistance(const myVec3 & v1,const myVec3 & v2)45 double MyDistance(const myVec3& v1, const myVec3& v2)
46 {
47 double rx = v1.x -v2.x;
48 double ry = v1.y -v2.y;
49 double rz = v1.z -v2.z;
50
51 return sqrt(rx*rx + ry*ry + rz*rz);
52 }
53
54 // Angle between two normalized vectors, in radians
AngleBetween(const myVec3 & v1,const myVec3 & v2)55 double AngleBetween(const myVec3& v1, const myVec3& v2)
56 {
57 double angle = MyDot(v1, v2);
58 // Prevent issues due to numerical precision
59 if (angle > 1.0)
60 angle = 1.0;
61 if (angle < -1.0)
62 angle = -1.0;
63
64 return acos(angle);
65 }
66
67 // Matrix 4x4 by 4x1 multiplication
68 // Attention: No bounds check!
MyMatMul4x1(const double * m1,const myVec4 & v)69 myVec4 MyMatMul4x1(const double *m1, const myVec4& v)
70 {
71 myVec4 mmv;
72 mmv.x = m1[0] * v.x + m1[4] * v.y + m1[8] * v.z + m1[12] * v.w ;
73 mmv.y = m1[1] * v.x + m1[5] * v.y + m1[9] * v.z + m1[13] * v.w ;
74 mmv.z = m1[2] * v.x + m1[6] * v.y + m1[10] * v.z + m1[14] * v.w ;
75 mmv.w = m1[3] * v.x + m1[7] * v.y + m1[11] * v.z + m1[15] * v.w ;
76
77 return mmv;
78 }
79
80 // Matrix 4x4 multiplication
81 // Attention: No bounds check!
MyMatMul4x4(const double * m1,const double * m2,double * mm)82 void MyMatMul4x4(const double *m1, const double *m2, double* mm)
83 {
84 mm[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3] ;
85 mm[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3] ;
86 mm[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3] ;
87 mm[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3] ;
88 mm[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7] ;
89 mm[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7] ;
90 mm[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7] ;
91 mm[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7] ;
92 mm[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11] ;
93 mm[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11] ;
94 mm[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11] ;
95 mm[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11] ;
96 mm[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15] ;
97 mm[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15] ;
98 mm[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15] ;
99 mm[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15] ;
100 }
101
102 // Matrix 4x4 inverse. Returns the determinant.
103 // Attention: No bounds check!
104 // Method used is "adjugate matrix" with "cofactors".
105 // A faster method, such as "LU decomposition", isn't much faster than this code.
MyMatInverse(const double * m,double * minv)106 double MyMatInverse(const double *m, double *minv)
107 {
108 double det;
109 double cof[16], sdt[19];
110
111 // The 2x2 determinants used for cofactors
112 sdt[0] = m[10] * m[15] - m[14] * m[11] ;
113 sdt[1] = m[9] * m[15] - m[13] * m[11] ;
114 sdt[2] = m[9] * m[14] - m[13] * m[10] ;
115 sdt[3] = m[8] * m[15] - m[12] * m[11] ;
116 sdt[4] = m[8] * m[14] - m[12] * m[10] ;
117 sdt[5] = m[8] * m[13] - m[12] * m[9] ;
118 sdt[6] = m[6] * m[15] - m[14] * m[7] ;
119 sdt[7] = m[5] * m[15] - m[13] * m[7] ;
120 sdt[8] = m[5] * m[14] - m[13] * m[6] ;
121 sdt[9] = m[4] * m[15] - m[12] * m[7] ;
122 sdt[10] = m[4] * m[14] - m[12] * m[6] ;
123 sdt[11] = m[5] * m[15] - m[13] * m[7] ;
124 sdt[12] = m[4] * m[13] - m[12] * m[5] ;
125 sdt[13] = m[6] * m[11] - m[10] * m[7] ;
126 sdt[14] = m[5] * m[11] - m[9] * m[7] ;
127 sdt[15] = m[5] * m[10] - m[9] * m[6] ;
128 sdt[16] = m[4] * m[11] - m[8] * m[7] ;
129 sdt[17] = m[4] * m[10] - m[8] * m[6] ;
130 sdt[18] = m[4] * m[9] - m[8] * m[5] ;
131 // The cofactors, transposed
132 cof[0] = m[5] * sdt[0] - m[6] * sdt[1] + m[7] * sdt[2] ;
133 cof[1] = - m[1] * sdt[0] + m[2] * sdt[1] - m[3] * sdt[2] ;
134 cof[2] = m[1] * sdt[6] - m[2] * sdt[7] + m[3] * sdt[8] ;
135 cof[3] = - m[1] * sdt[13] + m[2] * sdt[14] - m[3] * sdt[15] ;
136 cof[4] = - m[4] * sdt[0] + m[6] * sdt[3] - m[7] * sdt[4] ;
137 cof[5] = m[0] * sdt[0] - m[2] * sdt[3] + m[3] * sdt[4] ;
138 cof[6] = - m[0] * sdt[6] + m[2] * sdt[9] - m[3] * sdt[10] ;
139 cof[7] = m[0] * sdt[13] - m[2] * sdt[16] + m[3] * sdt[17] ;
140 cof[8] = m[4] * sdt[1] - m[5] * sdt[3] + m[7] * sdt[5] ;
141 cof[9] = - m[0] * sdt[1] + m[1] * sdt[3] - m[3] * sdt[5] ;
142 cof[10] = m[0] * sdt[11] - m[1] * sdt[9] + m[3] * sdt[12] ;
143 cof[11] = - m[0] * sdt[14] + m[1] * sdt[16] - m[3] * sdt[18] ;
144 cof[12] = - m[4] * sdt[2] + m[5] * sdt[4] - m[6] * sdt[5] ;
145 cof[13] = m[0] * sdt[2] - m[1] * sdt[4] + m[2] * sdt[5] ;
146 cof[14] = - m[0] * sdt[8] + m[1] * sdt[10] - m[2] * sdt[12] ;
147 cof[15] = m[0] * sdt[15] - m[1] * sdt[17] + m[2] * sdt[18] ;
148
149 det = m[0] * cof[0] + m[1] * cof[4] + m[2] * cof[8] + m[3] * cof[12] ;
150
151 if ( fabs(det) > 10E-9 ) // Some precision value
152 {
153 double invdet = 1.0 / det;
154 for (int i = 0; i < 16; ++i)
155 minv[i] = cof[i] * invdet;
156 }
157 else
158 {
159 // Enable comparison with 0
160 det = 0.0;
161 }
162
163 return det;
164 }
165
166 // Matrix of rotation around an axis in the origin.
167 // angle is positive if follows axis (right-hand rule)
168 // Attention: No bounds check!
MyRotate(const myVec3 & axis,double angle,double * mrot)169 void MyRotate(const myVec3& axis, double angle, double *mrot)
170 {
171 double c = cos(angle);
172 double s = sin(angle);
173 double t = 1.0 - c;
174
175 // Normalize the axis vector
176 myVec3 uv = MyNormalize(axis);
177
178 // Store the matrix in column order
179 mrot[0] = t * uv.x * uv.x + c ;
180 mrot[1] = t * uv.x * uv.y + s * uv.z ;
181 mrot[2] = t * uv.x * uv.z - s * uv.y ;
182 mrot[3] = 0.0 ;
183 mrot[4] = t * uv.y * uv.x - s * uv.z ;
184 mrot[5] = t * uv.y * uv.y + c ;
185 mrot[6] = t * uv.y * uv.z + s * uv.x ;
186 mrot[7] = 0.0 ;
187 mrot[8] = t * uv.z * uv.x + s * uv.y ;
188 mrot[9] = t * uv.z * uv.y - s * uv.x ;
189 mrot[10] = t * uv.z * uv.z + c ;
190 mrot[11] = 0.0 ;
191 mrot[12] = mrot[13] = mrot[14] = 0.0 ;
192 mrot[15] = 1.0 ;
193 }
194
195 // Matrix for defining the viewing transformation
196 // Attention: No bounds check!
197 // Unchecked conditions:
198 // camPos != targ && camUp != {0,0,0}
199 // camUo can't be parallel to camPos - targ
MyLookAt(const myVec3 & camPos,const myVec3 & camUp,const myVec3 & targ,double * mt)200 void MyLookAt(const myVec3& camPos, const myVec3& camUp, const myVec3& targ, double *mt)
201 {
202 myVec3 tc = MyNormalize(targ - camPos);
203 myVec3 up = MyNormalize(camUp);
204 // Normalize tc x up for the case where up is not perpendicular to tc
205 myVec3 s = MyNormalize(MyCross(tc, up));
206 myVec3 u = MyNormalize(MyCross(s, tc)); //Normalize to improve accuracy
207
208 // Store the matrix in column order
209 mt[0] = s.x ;
210 mt[1] = u.x ;
211 mt[2] = - tc.x ;
212 mt[3] = 0.0 ;
213 mt[4] = s.y ;
214 mt[5] = u.y ;
215 mt[6] = - tc.y ;
216 mt[7] = 0.0 ;
217 mt[8] = s.z ;
218 mt[9] = u.z ;
219 mt[10] = - tc.z ;
220 mt[11] = 0.0 ;
221 mt[12] = - MyDot(s, camPos) ;
222 mt[13] = - MyDot(u, camPos) ;
223 mt[14] = MyDot(tc, camPos) ;
224 mt[15] = 1.0 ;
225 }
226
227 // Matrix for defining the perspective projection with symmetric frustum
228 // From camera coordinates to canonical (2x2x2 cube) coordinates.
229 // Attention: No bounds check!
230 // Unchecked conditions: fov > 0 && zNear > 0 && zFar > zNear && aspect > 0
MyPerspective(double fov,double aspect,double zNear,double zFar,double * mp)231 void MyPerspective(double fov, double aspect, double zNear, double zFar, double *mp)
232 {
233 double f = 1.0 / tan(fov / 2.0);
234
235 // Store the matrix in column order
236 mp[0] = f / aspect ;
237 mp[1] = mp[2] = mp[3] = 0.0 ;
238 mp[4] = 0.0 ;
239 mp[5] = f ;
240 mp[6] = mp[7] = 0.0 ;
241 mp[8] = mp[9] = 0.0 ;
242 mp[10] = (zNear + zFar) / (zNear - zFar) ;
243 mp[11] = -1.0 ;
244 mp[12] = mp[13] = 0.0 ;
245 mp[14] = 2.0 * zNear * zFar / (zNear - zFar) ;
246 mp[15] = 0.0 ;
247 }
248
249 // Matrix for defining the orthogonal projection with symmetric frustum
250 // From camera coordinates to canonical (2x2x2 cube) coordinates.
251 // Attention: No bounds check!
252 // Unchecked conditions: left != right && bottom != top && zNear != zFar
MyOrtho(double left,double right,double bottom,double top,double zNear,double zFar,double * mo)253 void MyOrtho(double left, double right, double bottom, double top,
254 double zNear, double zFar, double *mo)
255 {
256 // Store the matrix in column order
257 mo[0] = 2.0 / (right - left) ;
258 mo[1] = mo[2] = mo[3] = mo[4] = 0.0 ;
259 mo[5] = 2.0 / (top - bottom) ;
260 mo[6] = mo[7] = mo[8] = mo[9] = 0.0 ;
261 mo[10] = 2.0 / (zNear - zFar) ;
262 mo[11] = 0.0 ;
263 mo[12] = -(right + left) / (right - left) ;
264 mo[13] = -(top + bottom) / (top - bottom) ;
265 mo[14] = (zNear + zFar) / (zNear - zFar) ;
266 mo[15] = 1.0 ;
267 }
268
269