| 265 | } |
| 266 | |
| 267 | void Matrix4D::rotLine(const Vector3d& vec, double fAngle) |
| 268 | { |
| 269 | // **** algorithm was taken from a math book |
| 270 | Matrix4D clMA; |
| 271 | Matrix4D clMB; |
| 272 | Matrix4D clMC; |
| 273 | Matrix4D clMRot; |
| 274 | Vector3d clRotAxis(vec); |
| 275 | double fcos {}; |
| 276 | double fsin {}; |
| 277 | |
| 278 | // set all entries to "0" |
| 279 | clMA.nullify(); |
| 280 | clMB.nullify(); |
| 281 | clMC.nullify(); |
| 282 | |
| 283 | // ** normalize the rotation axis |
| 284 | clRotAxis.Normalize(); |
| 285 | |
| 286 | // ** set the rotation matrix (formula taken from a math book) */ |
| 287 | fcos = cos(fAngle); |
| 288 | fsin = sin(fAngle); |
| 289 | |
| 290 | clMA.dMtrx4D[0][0] = (1 - fcos) * clRotAxis.x * clRotAxis.x; |
| 291 | clMA.dMtrx4D[0][1] = (1 - fcos) * clRotAxis.x * clRotAxis.y; |
| 292 | clMA.dMtrx4D[0][2] = (1 - fcos) * clRotAxis.x * clRotAxis.z; |
| 293 | clMA.dMtrx4D[1][0] = (1 - fcos) * clRotAxis.x * clRotAxis.y; |
| 294 | clMA.dMtrx4D[1][1] = (1 - fcos) * clRotAxis.y * clRotAxis.y; |
| 295 | clMA.dMtrx4D[1][2] = (1 - fcos) * clRotAxis.y * clRotAxis.z; |
| 296 | clMA.dMtrx4D[2][0] = (1 - fcos) * clRotAxis.x * clRotAxis.z; |
| 297 | clMA.dMtrx4D[2][1] = (1 - fcos) * clRotAxis.y * clRotAxis.z; |
| 298 | clMA.dMtrx4D[2][2] = (1 - fcos) * clRotAxis.z * clRotAxis.z; |
| 299 | |
| 300 | clMB.dMtrx4D[0][0] = fcos; |
| 301 | clMB.dMtrx4D[1][1] = fcos; |
| 302 | clMB.dMtrx4D[2][2] = fcos; |
| 303 | |
| 304 | clMC.dMtrx4D[0][1] = -fsin * clRotAxis.z; |
| 305 | clMC.dMtrx4D[0][2] = fsin * clRotAxis.y; |
| 306 | clMC.dMtrx4D[1][0] = fsin * clRotAxis.z; |
| 307 | clMC.dMtrx4D[1][2] = -fsin * clRotAxis.x; |
| 308 | clMC.dMtrx4D[2][0] = -fsin * clRotAxis.y; |
| 309 | clMC.dMtrx4D[2][1] = fsin * clRotAxis.x; |
| 310 | |
| 311 | for (int i = 0; i < 3; i++) { |
| 312 | for (int j = 0; j < 3; j++) { |
| 313 | clMRot.dMtrx4D[i][j] = clMA.dMtrx4D[i][j] + clMB.dMtrx4D[i][j] + clMC.dMtrx4D[i][j]; |
| 314 | } |
| 315 | } |
| 316 | |
| 317 | (*this) = clMRot * (*this); |
| 318 | } |
| 319 | |
| 320 | void Matrix4D::rotLine(const Vector3f& vec, float fAngle) |
| 321 | { |