1 | /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*- */ |
---|
2 | /* |
---|
3 | * Copyright (c) 2004-2013 HUBzero Foundation, LLC |
---|
4 | * |
---|
5 | * Author: Insoo Woo <iwoo@purdue.edu> |
---|
6 | */ |
---|
7 | |
---|
8 | #include <math.h> |
---|
9 | #include <memory.h> |
---|
10 | |
---|
11 | using namespace vrmath; |
---|
12 | |
---|
13 | #if 0 |
---|
14 | Vector3f calcNormal(const Vector3f& v1, const Vector3f& v2, const Vector3f& v3) |
---|
15 | { |
---|
16 | Vector3f temp; |
---|
17 | |
---|
18 | float a[3], b[3]; |
---|
19 | |
---|
20 | a[0] = v2.x - v1.x; |
---|
21 | a[1] = v2.y - v1.y; |
---|
22 | a[2] = v2.z - v1.z; |
---|
23 | b[0] = v3.x - v1.x; |
---|
24 | b[1] = v3.y - v1.y; |
---|
25 | b[2] = v3.z - v1.z; |
---|
26 | |
---|
27 | temp.x = a[1] * b[2] - a[2] * b[1]; |
---|
28 | temp.y = a[2] * b[0] - a[0] * b[2]; |
---|
29 | temp.z = a[0] * b[1] - a[1] * b[0]; |
---|
30 | |
---|
31 | float leng = (float)sqrt(temp.x * temp.x + temp.y * temp.y + temp.z * temp.z); |
---|
32 | |
---|
33 | if (leng != 0.0f) { |
---|
34 | temp.x /= leng; |
---|
35 | temp.y /= leng; |
---|
36 | temp.z /= leng; |
---|
37 | } |
---|
38 | else temp.set(-1.0f, 0.0f, 0.0f); |
---|
39 | |
---|
40 | return temp; |
---|
41 | } |
---|
42 | #endif |
---|
43 | |
---|
44 | static void __gluMakeIdentityd(float m[16]) |
---|
45 | { |
---|
46 | m[0+4*0] = 1; m[0+4*1] = 0; m[0+4*2] = 0; m[0+4*3] = 0; |
---|
47 | m[1+4*0] = 0; m[1+4*1] = 1; m[1+4*2] = 0; m[1+4*3] = 0; |
---|
48 | m[2+4*0] = 0; m[2+4*1] = 0; m[2+4*2] = 1; m[2+4*3] = 0; |
---|
49 | m[3+4*0] = 0; m[3+4*1] = 0; m[3+4*2] = 0; m[3+4*3] = 1; |
---|
50 | } |
---|
51 | |
---|
52 | static int __gluInvertMatrixd(const float m[16], float invOut[16]) |
---|
53 | { |
---|
54 | float inv[16], det; |
---|
55 | int i; |
---|
56 | |
---|
57 | inv[0] = m[5]*m[10]*m[15] - m[5]*m[11]*m[14] - m[9]*m[6]*m[15] |
---|
58 | + m[9]*m[7]*m[14] + m[13]*m[6]*m[11] - m[13]*m[7]*m[10]; |
---|
59 | inv[4] = -m[4]*m[10]*m[15] + m[4]*m[11]*m[14] + m[8]*m[6]*m[15] |
---|
60 | - m[8]*m[7]*m[14] - m[12]*m[6]*m[11] + m[12]*m[7]*m[10]; |
---|
61 | inv[8] = m[4]*m[9]*m[15] - m[4]*m[11]*m[13] - m[8]*m[5]*m[15] |
---|
62 | + m[8]*m[7]*m[13] + m[12]*m[5]*m[11] - m[12]*m[7]*m[9]; |
---|
63 | inv[12] = -m[4]*m[9]*m[14] + m[4]*m[10]*m[13] + m[8]*m[5]*m[14] |
---|
64 | - m[8]*m[6]*m[13] - m[12]*m[5]*m[10] + m[12]*m[6]*m[9]; |
---|
65 | inv[1] = -m[1]*m[10]*m[15] + m[1]*m[11]*m[14] + m[9]*m[2]*m[15] |
---|
66 | - m[9]*m[3]*m[14] - m[13]*m[2]*m[11] + m[13]*m[3]*m[10]; |
---|
67 | inv[5] = m[0]*m[10]*m[15] - m[0]*m[11]*m[14] - m[8]*m[2]*m[15] |
---|
68 | + m[8]*m[3]*m[14] + m[12]*m[2]*m[11] - m[12]*m[3]*m[10]; |
---|
69 | inv[9] = -m[0]*m[9]*m[15] + m[0]*m[11]*m[13] + m[8]*m[1]*m[15] |
---|
70 | - m[8]*m[3]*m[13] - m[12]*m[1]*m[11] + m[12]*m[3]*m[9]; |
---|
71 | inv[13] = m[0]*m[9]*m[14] - m[0]*m[10]*m[13] - m[8]*m[1]*m[14] |
---|
72 | + m[8]*m[2]*m[13] + m[12]*m[1]*m[10] - m[12]*m[2]*m[9]; |
---|
73 | inv[2] = m[1]*m[6]*m[15] - m[1]*m[7]*m[14] - m[5]*m[2]*m[15] |
---|
74 | + m[5]*m[3]*m[14] + m[13]*m[2]*m[7] - m[13]*m[3]*m[6]; |
---|
75 | inv[6] = -m[0]*m[6]*m[15] + m[0]*m[7]*m[14] + m[4]*m[2]*m[15] |
---|
76 | - m[4]*m[3]*m[14] - m[12]*m[2]*m[7] + m[12]*m[3]*m[6]; |
---|
77 | inv[10] = m[0]*m[5]*m[15] - m[0]*m[7]*m[13] - m[4]*m[1]*m[15] |
---|
78 | + m[4]*m[3]*m[13] + m[12]*m[1]*m[7] - m[12]*m[3]*m[5]; |
---|
79 | inv[14] = -m[0]*m[5]*m[14] + m[0]*m[6]*m[13] + m[4]*m[1]*m[14] |
---|
80 | - m[4]*m[2]*m[13] - m[12]*m[1]*m[6] + m[12]*m[2]*m[5]; |
---|
81 | inv[3] = -m[1]*m[6]*m[11] + m[1]*m[7]*m[10] + m[5]*m[2]*m[11] |
---|
82 | - m[5]*m[3]*m[10] - m[9]*m[2]*m[7] + m[9]*m[3]*m[6]; |
---|
83 | inv[7] = m[0]*m[6]*m[11] - m[0]*m[7]*m[10] - m[4]*m[2]*m[11] |
---|
84 | + m[4]*m[3]*m[10] + m[8]*m[2]*m[7] - m[8]*m[3]*m[6]; |
---|
85 | inv[11] = -m[0]*m[5]*m[11] + m[0]*m[7]*m[9] + m[4]*m[1]*m[11] |
---|
86 | - m[4]*m[3]*m[9] - m[8]*m[1]*m[7] + m[8]*m[3]*m[5]; |
---|
87 | inv[15] = m[0]*m[5]*m[10] - m[0]*m[6]*m[9] - m[4]*m[1]*m[10] |
---|
88 | + m[4]*m[2]*m[9] + m[8]*m[1]*m[6] - m[8]*m[2]*m[5]; |
---|
89 | |
---|
90 | det = m[0]*inv[0] + m[1]*inv[4] + m[2]*inv[8] + m[3]*inv[12]; |
---|
91 | |
---|
92 | if (det == 0) |
---|
93 | return 0; |
---|
94 | |
---|
95 | det = 1.0f / det; |
---|
96 | |
---|
97 | for (i = 0; i < 16; i++) |
---|
98 | invOut[i] = inv[i] * det; |
---|
99 | |
---|
100 | return 1; |
---|
101 | } |
---|
102 | |
---|
103 | static void __gluMultMatrixVecd(const float matrix[16], const float in[4], |
---|
104 | float out[4]) |
---|
105 | { |
---|
106 | int i; |
---|
107 | |
---|
108 | for (i=0; i<4; i++) { |
---|
109 | out[i] = |
---|
110 | in[0] * matrix[0*4+i] + |
---|
111 | in[1] * matrix[1*4+i] + |
---|
112 | in[2] * matrix[2*4+i] + |
---|
113 | in[3] * matrix[3*4+i]; |
---|
114 | } |
---|
115 | } |
---|
116 | |
---|
117 | static void __gluMultMatricesd(const float a[16], const float b[16], |
---|
118 | float r[16]) |
---|
119 | { |
---|
120 | int i, j; |
---|
121 | |
---|
122 | for (i = 0; i < 4; i++) { |
---|
123 | for (j = 0; j < 4; j++) { |
---|
124 | r[i*4+j] = |
---|
125 | a[i*4+0]*b[0*4+j] + |
---|
126 | a[i*4+1]*b[1*4+j] + |
---|
127 | a[i*4+2]*b[2*4+j] + |
---|
128 | a[i*4+3]*b[3*4+j]; |
---|
129 | } |
---|
130 | } |
---|
131 | } |
---|
132 | |
---|
133 | int unproject(float winx, float winy, float winz, |
---|
134 | const float modelMatrix[16], |
---|
135 | const float projMatrix[16], |
---|
136 | const int viewport[4], |
---|
137 | float *objx, float *objy, float *objz) |
---|
138 | { |
---|
139 | float finalMatrix[16]; |
---|
140 | float in[4]; |
---|
141 | float out[4]; |
---|
142 | |
---|
143 | __gluMultMatricesd(modelMatrix, projMatrix, finalMatrix); |
---|
144 | if (!__gluInvertMatrixd(finalMatrix, finalMatrix)) return(0); |
---|
145 | |
---|
146 | in[0]=winx; |
---|
147 | in[1]=winy; |
---|
148 | in[2]=winz; |
---|
149 | in[3]=1.0; |
---|
150 | |
---|
151 | // Map x and y from window coordinates |
---|
152 | in[0] = (in[0] - viewport[0]) / viewport[2]; |
---|
153 | in[1] = (in[1] - viewport[1]) / viewport[3]; |
---|
154 | |
---|
155 | // Map to range -1 to 1 |
---|
156 | in[0] = in[0] * 2 - 1; |
---|
157 | in[1] = in[1] * 2 - 1; |
---|
158 | in[2] = in[2] * 2 - 1; |
---|
159 | |
---|
160 | __gluMultMatrixVecd(finalMatrix, in, out); |
---|
161 | if (out[3] == 0.0) return(0); |
---|
162 | out[0] /= out[3]; |
---|
163 | out[1] /= out[3]; |
---|
164 | out[2] /= out[3]; |
---|
165 | *objx = out[0]; |
---|
166 | *objy = out[1]; |
---|
167 | *objz = out[2]; |
---|
168 | return(1); |
---|
169 | } |
---|
170 | |
---|
171 | #define __glPi 3.14159265358979323846 |
---|
172 | |
---|
173 | void perspective(float fovy, float aspect, float zNear, float zFar, float* matrix) |
---|
174 | { |
---|
175 | float m[4][4]; |
---|
176 | float sine, cotangent, deltaZ; |
---|
177 | float radians = (float) (fovy / 2.0 * __glPi / 180.0); |
---|
178 | |
---|
179 | deltaZ = zFar - zNear; |
---|
180 | sine = sin(radians); |
---|
181 | if ((deltaZ == 0) || (sine == 0) || (aspect == 0)) { |
---|
182 | return; |
---|
183 | } |
---|
184 | cotangent = cos(radians) / sine; |
---|
185 | |
---|
186 | __gluMakeIdentityd(&m[0][0]); |
---|
187 | m[0][0] = cotangent / aspect; |
---|
188 | m[1][1] = cotangent; |
---|
189 | m[2][2] = -(zFar + zNear) / deltaZ; |
---|
190 | m[2][3] = -1; |
---|
191 | m[3][2] = -2 * zNear * zFar / deltaZ; |
---|
192 | m[3][3] = 0; |
---|
193 | //glMultMatrixd(&m[0][0]); |
---|
194 | memcpy(matrix, &m[0][0], sizeof(float) * 16); |
---|
195 | } |
---|