#
root/OpenSceneGraph/trunk/src/osgPlugins/3ds/lib3ds/lib3ds_matrix.c
@
10853

Revision 10853, 10.7 kB (checked in by robert, 5 years ago) |
---|

Rev | Line | |
---|---|---|

[10853] | 1 | /* |

2 | Copyright (C) 1996-2008 by Jan Eric Kyprianidis <www.kyprianidis.com> | |

3 | All rights reserved. | |

4 | | |

5 | This program is free software: you can redistribute it and/or modify | |

6 | it under the terms of the GNU Lesser General Public License as published | |

7 | by the Free Software Foundation, either version 2.1 of the License, or | |

8 | (at your option) any later version. | |

9 | ||

10 | Thisprogram is distributed in the hope that it will be useful, | |

11 | but WITHOUT ANY WARRANTY; without even the implied warranty of | |

12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |

13 | GNU Lesser General Public License for more details. | |

14 | | |

15 | You should have received a copy of the GNU Lesser General Public License | |

16 | along with this program; If not, see <http://www.gnu.org/licenses/>. | |

17 | */ | |

18 | #include "lib3ds_impl.h" | |

19 | ||

20 | ||

21 | /*! | |

22 | * Clear a matrix to all zeros. | |

23 | * | |

24 | * \param m Matrix to be cleared. | |

25 | */ | |

26 | void | |

27 | lib3ds_matrix_zero(float m[4][4]) { | |

28 | int i, j; | |

29 | ||

30 | for (i = 0; i < 4; i++) { | |

31 | for (j = 0; j < 4; j++) m[i][j] = 0.0f; | |

32 | } | |

33 | } | |

34 | ||

35 | ||

36 | /*! | |

37 | * Set a matrix to identity. | |

38 | * | |

39 | * \param m Matrix to be set. | |

40 | */ | |

41 | void | |

42 | lib3ds_matrix_identity(float m[4][4]) { | |

43 | int i, j; | |

44 | ||

45 | for (i = 0; i < 4; i++) { | |

46 | for (j = 0; j < 4; j++) m[i][j] = 0.0; | |

47 | } | |

48 | for (i = 0; i < 4; i++) m[i][i] = 1.0; | |

49 | } | |

50 | ||

51 | ||

52 | /*! | |

53 | * Copy a matrix. | |

54 | */ | |

55 | void | |

56 | lib3ds_matrix_copy(float dest[4][4], float src[4][4]) { | |

57 | memcpy(dest, src, 16 * sizeof(float)); | |

58 | } | |

59 | ||

60 | ||

61 | /*! | |

62 | * Negate a matrix -- all elements negated. | |

63 | */ | |

64 | void | |

65 | lib3ds_matrix_neg(float m[4][4]) { | |

66 | int i, j; | |

67 | ||

68 | for (j = 0; j < 4; j++) { | |

69 | for (i = 0; i < 4; i++) { | |

70 | m[j][i] = -m[j][i]; | |

71 | } | |

72 | } | |

73 | } | |

74 | ||

75 | ||

76 | /*! | |

77 | * Transpose a matrix in place. | |

78 | */ | |

79 | void | |

80 | lib3ds_matrix_transpose(float m[4][4]) { | |

81 | int i, j; | |

82 | float swp; | |

83 | ||

84 | for (j = 0; j < 4; j++) { | |

85 | for (i = j + 1; i < 4; i++) { | |

86 | swp = m[j][i]; | |

87 | m[j][i] = m[i][j]; | |

88 | m[i][j] = swp; | |

89 | } | |

90 | } | |

91 | } | |

92 | ||

93 | ||

94 | /*! | |

95 | * Add two matrices. | |

96 | */ | |

97 | void | |

98 | lib3ds_matrix_add(float m[4][4], float a[4][4], float b[4][4]) { | |

99 | int i, j; | |

100 | ||

101 | for (j = 0; j < 4; j++) { | |

102 | for (i = 0; i < 4; i++) { | |

103 | m[j][i] = a[j][i] + b[j][i]; | |

104 | } | |

105 | } | |

106 | } | |

107 | ||

108 | ||

109 | /*! | |

110 | * Subtract two matrices. | |

111 | * | |

112 | * \param m Result. | |

113 | * \param a Addend. | |

114 | * \param b Minuend. | |

115 | */ | |

116 | void | |

117 | lib3ds_matrix_sub(float m[4][4], float a[4][4], float b[4][4]) { | |

118 | int i, j; | |

119 | ||

120 | for (j = 0; j < 4; j++) { | |

121 | for (i = 0; i < 4; i++) { | |

122 | m[j][i] = a[j][i] - b[j][i]; | |

123 | } | |

124 | } | |

125 | } | |

126 | ||

127 | ||

128 | /*! | |

129 | * Multiplies a matrix by a second one (m = m * n). | |

130 | */ | |

131 | void | |

132 | lib3ds_matrix_mult(float m[4][4], float a[4][4], float b[4][4]) { | |

133 | float tmp[4][4]; | |

134 | int i, j, k; | |

135 | float ab; | |

136 | ||

137 | memcpy(tmp, a, 16 * sizeof(float)); | |

138 | for (j = 0; j < 4; j++) { | |

139 | for (i = 0; i < 4; i++) { | |

140 | ab = 0.0f; | |

141 | for (k = 0; k < 4; k++) ab += tmp[k][i] * b[j][k]; | |

142 | m[j][i] = ab; | |

143 | } | |

144 | } | |

145 | } | |

146 | ||

147 | ||

148 | /*! | |

149 | * Multiply a matrix by a scalar. | |

150 | * | |

151 | * \param m Matrix to be set. | |

152 | * \param k Scalar. | |

153 | */ | |

154 | void | |

155 | lib3ds_matrix_scalar(float m[4][4], float k) { | |

156 | int i, j; | |

157 | ||

158 | for (j = 0; j < 4; j++) { | |

159 | for (i = 0; i < 4; i++) { | |

160 | m[j][i] *= k; | |

161 | } | |

162 | } | |

163 | } | |

164 | ||

165 | ||

166 | static float | |

167 | det2x2( | |

168 | float a, float b, | |

169 | float c, float d) { | |

170 | return((a)*(d) - (b)*(c)); | |

171 | } | |

172 | ||

173 | ||

174 | static float | |

175 | det3x3( | |

176 | float a1, float a2, float a3, | |

177 | float b1, float b2, float b3, | |

178 | float c1, float c2, float c3) { | |

179 | return( | |

180 | a1*det2x2(b2, b3, c2, c3) - | |

181 | b1*det2x2(a2, a3, c2, c3) + | |

182 | c1*det2x2(a2, a3, b2, b3) | |

183 | ); | |

184 | } | |

185 | ||

186 | ||

187 | /*! | |

188 | * Find determinant of a matrix. | |

189 | */ | |

190 | float | |

191 | lib3ds_matrix_det(float m[4][4]) { | |

192 | float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; | |

193 | ||

194 | a1 = m[0][0]; | |

195 | b1 = m[1][0]; | |

196 | c1 = m[2][0]; | |

197 | d1 = m[3][0]; | |

198 | a2 = m[0][1]; | |

199 | b2 = m[1][1]; | |

200 | c2 = m[2][1]; | |

201 | d2 = m[3][1]; | |

202 | a3 = m[0][2]; | |

203 | b3 = m[1][2]; | |

204 | c3 = m[2][2]; | |

205 | d3 = m[3][2]; | |

206 | a4 = m[0][3]; | |

207 | b4 = m[1][3]; | |

208 | c4 = m[2][3]; | |

209 | d4 = m[3][3]; | |

210 | return( | |

211 | a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - | |

212 | b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) + | |

213 | c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - | |

214 | d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4) | |

215 | ); | |

216 | } | |

217 | ||

218 | ||

219 | /*! | |

220 | * Invert a matrix in place. | |

221 | * | |

222 | * \param m Matrix to invert. | |

223 | * | |

224 | * \return LIB3DS_TRUE on success, LIB3DS_FALSE on failure. | |

225 | * | |

226 | * GGemsII, K.Wu, Fast Matrix Inversion | |

227 | */ | |

228 | int | |

229 | lib3ds_matrix_inv(float m[4][4]) { | |

230 | int i, j, k; | |

231 | int pvt_i[4], pvt_j[4]; /* Locations of pivot elements */ | |

232 | float pvt_val; /* Value of current pivot element */ | |

233 | float hold; /* Temporary storage */ | |

234 | float determinat; | |

235 | ||

236 | determinat = 1.0f; | |

237 | for (k = 0; k < 4; k++) { | |

238 | /* Locate k'th pivot element */ | |

239 | pvt_val = m[k][k]; /* Initialize for search */ | |

240 | pvt_i[k] = k; | |

241 | pvt_j[k] = k; | |

242 | for (i = k; i < 4; i++) { | |

243 | for (j = k; j < 4; j++) { | |

244 | if (fabs(m[i][j]) > fabs(pvt_val)) { | |

245 | pvt_i[k] = i; | |

246 | pvt_j[k] = j; | |

247 | pvt_val = m[i][j]; | |

248 | } | |

249 | } | |

250 | } | |

251 | ||

252 | /* Product of pivots, gives determinant when finished */ | |

253 | determinat *= pvt_val; | |

254 | if (fabs(determinat) < LIB3DS_EPSILON) { | |

255 | return(FALSE); /* Matrix is singular (zero determinant) */ | |

256 | } | |

257 | ||

258 | /* "Interchange" rows (with sign change stuff) */ | |

259 | i = pvt_i[k]; | |

260 | if (i != k) { /* If rows are different */ | |

261 | for (j = 0; j < 4; j++) { | |

262 | hold = -m[k][j]; | |

263 | m[k][j] = m[i][j]; | |

264 | m[i][j] = hold; | |

265 | } | |

266 | } | |

267 | ||

268 | /* "Interchange" columns */ | |

269 | j = pvt_j[k]; | |

270 | if (j != k) { /* If columns are different */ | |

271 | for (i = 0; i < 4; i++) { | |

272 | hold = -m[i][k]; | |

273 | m[i][k] = m[i][j]; | |

274 | m[i][j] = hold; | |

275 | } | |

276 | } | |

277 | ||

278 | /* Divide column by minus pivot value */ | |

279 | for (i = 0; i < 4; i++) { | |

280 | if (i != k) m[i][k] /= (-pvt_val) ; | |

281 | } | |

282 | ||

283 | /* Reduce the matrix */ | |

284 | for (i = 0; i < 4; i++) { | |

285 | hold = m[i][k]; | |

286 | for (j = 0; j < 4; j++) { | |

287 | if (i != k && j != k) m[i][j] += hold * m[k][j]; | |

288 | } | |

289 | } | |

290 | ||

291 | /* Divide row by pivot */ | |

292 | for (j = 0; j < 4; j++) { | |

293 | if (j != k) m[k][j] /= pvt_val; | |

294 | } | |

295 | ||

296 | /* Replace pivot by reciprocal (at last we can touch it). */ | |

297 | m[k][k] = 1.0f / pvt_val; | |

298 | } | |

299 | ||

300 | /* That was most of the work, one final pass of row/column interchange */ | |

301 | /* to finish */ | |

302 | for (k = 4 - 2; k >= 0; k--) { /* Don't need to work with 1 by 1 corner*/ | |

303 | i = pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */ | |

304 | if (i != k) { /* If rows are different */ | |

305 | for (j = 0; j < 4; j++) { | |

306 | hold = m[k][j]; | |

307 | m[k][j] = -m[i][j]; | |

308 | m[i][j] = hold; | |

309 | } | |

310 | } | |

311 | ||

312 | j = pvt_i[k]; /* Columns to swap correspond to pivot ROW */ | |

313 | if (j != k) /* If columns are different */ | |

314 | for (i = 0; i < 4; i++) { | |

315 | hold = m[i][k]; | |

316 | m[i][k] = -m[i][j]; | |

317 | m[i][j] = hold; | |

318 | } | |

319 | } | |

320 | return(TRUE); | |

321 | } | |

322 | ||

323 | ||

324 | /*! | |

325 | * Apply a translation to a matrix. | |

326 | */ | |

327 | void | |

328 | lib3ds_matrix_translate(float m[4][4], float x, float y, float z) { | |

329 | int i; | |

330 | ||

331 | for (i = 0; i < 3; i++) { | |

332 | m[3][i] += m[0][i] * x + m[1][i] * y + m[2][i] * z; | |

333 | } | |

334 | } | |

335 | ||

336 | ||

337 | /*! | |

338 | * Apply scale factors to a matrix. | |

339 | */ | |

340 | void | |

341 | lib3ds_matrix_scale(float m[4][4], float x, float y, float z) { | |

342 | int i; | |

343 | ||

344 | for (i = 0; i < 4; i++) { | |

345 | m[0][i] *= x; | |

346 | m[1][i] *= y; | |

347 | m[2][i] *= z; | |

348 | } | |

349 | } | |

350 | ||

351 | ||

352 | /*! | |

353 | * Apply a rotation about an arbitrary axis to a matrix. | |

354 | */ | |

355 | void | |

356 | lib3ds_matrix_rotate_quat(float m[4][4], float q[4]) { | |

357 | float s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, l; | |

358 | float R[4][4]; | |

359 | ||

360 | l = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; | |

361 | if (fabs(l) < LIB3DS_EPSILON) { | |

362 | s = 1.0f; | |

363 | } else { | |

364 | s = 2.0f / l; | |

365 | } | |

366 | ||

367 | xs = q[0] * s; | |

368 | ys = q[1] * s; | |

369 | zs = q[2] * s; | |

370 | wx = q[3] * xs; | |

371 | wy = q[3] * ys; | |

372 | wz = q[3] * zs; | |

373 | xx = q[0] * xs; | |

374 | xy = q[0] * ys; | |

375 | xz = q[0] * zs; | |

376 | yy = q[1] * ys; | |

377 | yz = q[1] * zs; | |

378 | zz = q[2] * zs; | |

379 | ||

380 | R[0][0] = 1.0f - (yy + zz); | |

381 | R[1][0] = xy - wz; | |

382 | R[2][0] = xz + wy; | |

383 | R[0][1] = xy + wz; | |

384 | R[1][1] = 1.0f - (xx + zz); | |

385 | R[2][1] = yz - wx; | |

386 | R[0][2] = xz - wy; | |

387 | R[1][2] = yz + wx; | |

388 | R[2][2] = 1.0f - (xx + yy); | |

389 | R[3][0] = R[3][1] = R[3][2] = R[0][3] = R[1][3] = R[2][3] = 0.0f; | |

390 | R[3][3] = 1.0f; | |

391 | ||

392 | lib3ds_matrix_mult(m, m, R); | |

393 | } | |

394 | ||

395 | ||

396 | /*! | |

397 | * Apply a rotation about an arbitrary axis to a matrix. | |

398 | */ | |

399 | void | |

400 | lib3ds_matrix_rotate(float m[4][4], float angle, float ax, float ay, float az) { | |

401 | float q[4]; | |

402 | float axis[3]; | |

403 | ||

404 | lib3ds_vector_make(axis, ax, ay, az); | |

405 | lib3ds_quat_axis_angle(q, axis, angle); | |

406 | lib3ds_matrix_rotate_quat(m, q); | |

407 | } | |

408 | ||

409 | ||

410 | /*! | |

411 | * Compute a camera matrix based on position, target and roll. | |

412 | * | |

413 | * Generates a translate/rotate matrix that maps world coordinates | |

414 | * to camera coordinates. Resulting matrix does not include perspective | |

415 | * transform. | |

416 | * | |

417 | * \param matrix Destination matrix. | |

418 | * \param pos Camera position | |

419 | * \param tgt Camera target | |

420 | * \param roll Roll angle | |

421 | */ | |

422 | void | |

423 | lib3ds_matrix_camera(float matrix[4][4], float pos[3], float tgt[3], float roll) { | |

424 | float M[4][4]; | |

425 | float x[3], y[3], z[3]; | |

426 | ||

427 | lib3ds_vector_sub(y, tgt, pos); | |

428 | lib3ds_vector_normalize(y); | |

429 | ||

430 | if (y[0] != 0. || y[1] != 0) { | |

431 | z[0] = 0; | |

432 | z[1] = 0; | |

433 | z[2] = 1.0; | |

434 | } else { /* Special case: looking straight up or down z axis */ | |

435 | z[0] = -1.0; | |

436 | z[1] = 0; | |

437 | z[2] = 0; | |

438 | } | |

439 | ||

440 | lib3ds_vector_cross(x, y, z); | |

441 | lib3ds_vector_cross(z, x, y); | |

442 | lib3ds_vector_normalize(x); | |

443 | lib3ds_vector_normalize(z); | |

444 | ||

445 | lib3ds_matrix_identity(M); | |

446 | M[0][0] = x[0]; | |

447 | M[1][0] = x[1]; | |

448 | M[2][0] = x[2]; | |

449 | M[0][1] = y[0]; | |

450 | M[1][1] = y[1]; | |

451 | M[2][1] = y[2]; | |

452 | M[0][2] = z[0]; | |

453 | M[1][2] = z[1]; | |

454 | M[2][2] = z[2]; | |

455 | ||

456 | lib3ds_matrix_identity(matrix); | |

457 | lib3ds_matrix_rotate(matrix, roll, 0, 1, 0); | |

458 | lib3ds_matrix_mult(matrix, matrix, M); | |

459 | lib3ds_matrix_translate(matrix, -pos[0], -pos[1], -pos[2]); | |

460 | } | |

461 | ||

462 | ||

463 | ||

464 | ||

465 |

**Note:**See TracBrowser for help on using the browser.