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