SetAxisFromRotationMatrix sets this vector to be the axis (x, y, z) and angle (w) of a rotation specified the matrix m. Assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled).
(m *Matrix4)
| 509 | // SetAxisFromRotationMatrix sets this vector to be the axis (x, y, z) and angle (w) of a rotation specified the matrix m. |
| 510 | // Assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled). |
| 511 | func (v *Vector4) SetAxisFromRotationMatrix(m *Matrix4) *Vector4 { |
| 512 | |
| 513 | // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm |
| 514 | var angle, x, y, z float32 // variables for result |
| 515 | var epsilon float32 = 0.01 // margin to allow for rounding errors |
| 516 | var epsilon2 float32 = 0.1 // margin to distinguish between 0 and 180 degrees |
| 517 | |
| 518 | m11 := m[0] |
| 519 | m12 := m[4] |
| 520 | m13 := m[8] |
| 521 | m21 := m[1] |
| 522 | m22 := m[5] |
| 523 | m23 := m[9] |
| 524 | m31 := m[2] |
| 525 | m32 := m[6] |
| 526 | m33 := m[10] |
| 527 | |
| 528 | if (Abs(m12-m21) < epsilon) && (Abs(m13-m31) < epsilon) && (Abs(m23-m32) < epsilon) { |
| 529 | |
| 530 | // singularity found |
| 531 | // first check for identity matrix which must have +1 for all terms |
| 532 | // in leading diagonal and zero in other terms |
| 533 | |
| 534 | if (Abs(m12+m21) < epsilon2) && (Abs(m13+m31) < epsilon2) && (Abs(m23+m32) < epsilon2) && (Abs(m11+m22+m33-3) < epsilon2) { |
| 535 | |
| 536 | // v singularity is identity matrix so angle = 0 |
| 537 | |
| 538 | v.Set(1, 0, 0, 0) |
| 539 | |
| 540 | return v // zero angle, arbitrary axis |
| 541 | } |
| 542 | |
| 543 | // otherwise this singularity is angle = 180 |
| 544 | |
| 545 | angle = Pi |
| 546 | |
| 547 | var xx = (m11 + 1) / 2 |
| 548 | var yy = (m22 + 1) / 2 |
| 549 | var zz = (m33 + 1) / 2 |
| 550 | var xy = (m12 + m21) / 4 |
| 551 | var xz = (m13 + m31) / 4 |
| 552 | var yz = (m23 + m32) / 4 |
| 553 | |
| 554 | if (xx > yy) && (xx > zz) { // m11 is the largest diagonal term |
| 555 | |
| 556 | if xx < epsilon { |
| 557 | |
| 558 | x = 0 |
| 559 | y = 0.707106781 |
| 560 | z = 0.707106781 |
| 561 | |
| 562 | } else { |
| 563 | |
| 564 | x = Sqrt(xx) |
| 565 | y = xy / x |
| 566 | z = xz / x |
| 567 | |
| 568 | } |