-
Notifications
You must be signed in to change notification settings - Fork 22
/
Math.cpp
176 lines (146 loc) · 4.07 KB
/
Math.cpp
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
#include "StdAfx.h"
#pragma warning(disable: 4244)
long RandomLong(long min, long max)
{
float fraction = ((float)rand() / RAND_MAX);
fraction *= ((max - min) + 1);
long value = fraction + min;
if (value > max)
value = max;
return value;
}
float RandomFloat(float min, float max)
{
float fraction = ((float)rand() / RAND_MAX);
fraction *= (max - min);
float value = fraction + min;
return value;
}
float FindVectorZ(const Vector& p1, const Vector& p2, const Vector& p3, float x, float y )
{
Vector v1 = p3-p1;
Vector v2 = p2-p1;
Vector normal = CrossProduct( v1, v2 ).Normalize();
float poo = -( (normal.x * p1.x) + (normal.y * p1.y) + (normal.z * p1.z) );
float z = (-( (normal.x * x) + (normal.y * y) + poo) ) / normal.z;
return z;
}
/* Matrix courtesy of Asriel */
matrix::matrix() {
data[0][0] = 1.0f;
data[0][1] = 0.0f;
data[0][2] = 0.0f;
data[0][3] = 0.0f;
data[1][0] = 0.0f;
data[1][1] = 1.0f;
data[1][2] = 0.0f;
data[1][3] = 0.0f;
data[2][0] = 0.0f;
data[2][1] = 0.0f;
data[2][2] = 1.0f;
data[2][3] = 0.0f;
data[3][0] = 0.0f;
data[3][1] = 0.0f;
data[3][2] = 0.0f;
data[3][3] = 1.0f;
}
/* General definition */
void matrix::define(float xa, float xb, float xc, float xd, float ya, float yb, float yc, float yd,
float za, float zb, float zc, float zd) {
data[0][0] = xa;
data[0][1] = xb;
data[0][2] = xc;
data[0][3] = xd;
data[1][0] = ya;
data[1][1] = yb;
data[1][2] = yc;
data[1][3] = yd;
data[2][0] = za;
data[2][1] = zb;
data[2][2] = zc;
data[2][3] = zd;
}
/* Definition by Quaternion */
void matrix::defineByQuaternion(float qw, float qx, float qy, float qz) {
data[0][0] = 1 - (2 * (qy * qy)) - (2 * (qz * qz));
data[1][0] = (2 * qx * qy) - (2 * qw * qz);
data[2][0] = (2 * qx * qz) + (2 * qw * qy);
data[0][1] = (2 * qx * qy) + (2 * qw * qz);
data[1][1] = 1 - (2 * (qx * qx)) - (2 * (qz * qz));
data[2][1] = (2 * qy * qz) - (2 * qw * qx);
data[0][2] = (2 * qx * qz) - (2 * qw * qy);
data[1][2] = (2 * qy * qz) + (2 * qw * qx);
data[2][2] = 1 - (2 * (qx * qx)) - (2 * (qy * qy));
}
/* Definition by Eulerian angular rotation */
#pragma warning(disable : 4244)
void matrix::defineByRotation(float roll, float pitch, float yaw) {
double sr,sp,sy,cr,cp,cy;
matrix mp, my;
sr = sin(DEG2RAD(roll));
cr = cos(DEG2RAD(roll));
sp = sin(DEG2RAD(pitch));
cp = cos(DEG2RAD(pitch));
sy = sin(DEG2RAD(yaw));
cy = cos(DEG2RAD(yaw));
data[0][0] = data[1][1] = cr;
data[1][0] = -(data[0][1] = sr);
mp.data[1][1] = mp.data[2][2] = cp;
mp.data[1][2] = -(mp.data[2][1] = sp);
my.data[0][0] = my.data[2][2] = cy;
my.data[0][2] = -(my.data[2][0] = sy);
multiply(my);
multiply(mp);
}
void matrix::applyRotation(float roll, float pitch, float yaw) {
matrix mat;
mat.defineByRotation(roll, pitch, yaw);
multiply(mat);
}
void matrix::applyTranslation(float x, float y, float z) {
data[0][3] = x;
data[1][3] = y;
data[2][3] = z;
}
/* Apply this matrix to a vector */
void matrix::applyToVector(Vector &vect) {
float xo, yo, zo, wo;
xo = vect.x;
yo = vect.y;
zo = vect.z;
wo = 1.0f;
vect.x = xo * data[0][0] + yo * data[1][0] + zo * data[2][0] + wo * data[3][0];
vect.y = xo * data[0][1] + yo * data[1][1] + zo * data[2][1] + wo * data[3][0];
vect.z = xo * data[0][2] + yo * data[1][2] + zo * data[2][2] + wo * data[3][0];
}
void matrix::copy(matrix &dest) {
dest.define(data[0][0], data[0][1], data[0][2], data[0][3],
data[1][0], data[1][1], data[1][2], data[1][3],
data[2][0], data[2][1], data[2][2], data[2][3]);
}
void matrix::multiply(matrix second)
{
matrix temp;
copy(temp);
for (int j = 0; j < 4; j++)
for (int i = 0; i < 4; i++)
data[i][j] = temp.data[i][0] * second.data[0][j] +
temp.data[i][1] * second.data[1][j] +
temp.data[i][2] * second.data[2][j] +
temp.data[i][3] * second.data[3][j];
}
// Checks if the sum of two 32-bit values will overflow.
void inline WillOF( DWORD v1, DWORD v2, BOOL *bResult )
{
#ifdef _WIN32
__asm {
mov eax, DWORD PTR [v1]
add eax, DWORD PTR [v2]
xor eax, eax
adc eax, eax
mov DWORD PTR [bResult], eax
}
#else
*bResult = TRUE;
#endif
}