summaryrefslogtreecommitdiff
path: root/common/algebra.h
diff options
context:
space:
mode:
authorLeo2010-07-11 22:19:43 +0000
committerLeo2010-07-11 22:19:43 +0000
commit742ef27f8be974985399e5ddaf155862d7da5ad2 (patch)
tree204acc87f2bd1a4aac8ee943aec85207aa23e65a /common/algebra.h
parent66b3f4b8d9f1b606eb4a6e893db9f293cbeb95a3 (diff)
Updated Minifig Wizard to read settings from ini file.
git-svn-id: http://svn.leocad.org/branches/leocad-0.75@954 c7d43263-9d01-0410-8a33-9dba5d9f93d6
Diffstat (limited to 'common/algebra.h')
-rw-r--r--common/algebra.h93
1 files changed, 93 insertions, 0 deletions
diff --git a/common/algebra.h b/common/algebra.h
index 6655d3a..0e2be09 100644
--- a/common/algebra.h
+++ b/common/algebra.h
@@ -594,6 +594,99 @@ public:
*this = Mat;
}
+ Vector4 ToAxisAngle()
+ {
+ Matrix33 tmp(Vector3(m_Rows[0]).Normalize(), Vector3(m_Rows[1]).Normalize(), Vector3(m_Rows[2]).Normalize());
+
+ // Determinant should be 1 for rotation matrices.
+ float Determinant = tmp.m_Rows[0][0] * tmp.m_Rows[1][1] * tmp.m_Rows[2][2] + tmp.m_Rows[0][1] * tmp.m_Rows[1][2] * tmp.m_Rows[2][0] +
+ tmp.m_Rows[0][2] * tmp.m_Rows[1][0] * tmp.m_Rows[2][1] - tmp.m_Rows[0][0] * tmp.m_Rows[1][2] * tmp.m_Rows[2][1] -
+ tmp.m_Rows[0][1] * tmp.m_Rows[1][0] * tmp.m_Rows[2][2] - tmp.m_Rows[0][2] * tmp.m_Rows[1][1] * tmp.m_Rows[2][0];
+
+ if (Determinant < 0.0f)
+ tmp.m_Rows[0] *= -1.0f;
+
+ float Trace = tmp.m_Rows[0][0] + tmp.m_Rows[1][1] + tmp.m_Rows[2][2];
+ float Cos = 0.5f * (Trace - 1.0f);
+ Vector4 rot;
+
+ if (Cos < -1.0f)
+ Cos = -1.0f;
+ else if (Cos > 1.0f)
+ Cos = 1.0f;
+ rot[3] = acosf(Cos); // in [0,PI]
+
+ if (rot[3] > 0.01f)
+ {
+ if (fabsf(3.141592f - rot[3]) > 0.01f)
+ {
+ rot[0] = tmp.m_Rows[1][2] - tmp.m_Rows[2][1];
+ rot[1] = tmp.m_Rows[2][0] - tmp.m_Rows[0][2];
+ rot[2] = tmp.m_Rows[0][1] - tmp.m_Rows[1][0];
+
+ float inv = 1.0f / sqrtf(rot[0]*rot[0] + rot[1]*rot[1] + rot[2]*rot[2]);
+
+ rot[0] *= inv;
+ rot[1] *= inv;
+ rot[2] *= inv;
+ }
+ else
+ {
+ // angle is PI
+ float HalfInverse;
+ if (tmp.m_Rows[0][0] >= tmp.m_Rows[1][1])
+ {
+ // r00 >= r11
+ if (tmp.m_Rows[0][0] >= tmp.m_Rows[2][2])
+ {
+ // r00 is maximum diagonal term
+ rot[0] = 0.5f * sqrtf(tmp.m_Rows[0][0] - tmp.m_Rows[1][1] - tmp.m_Rows[2][2] + 1.0f);
+ HalfInverse = 0.5f / rot[0];
+ rot[1] = HalfInverse * tmp.m_Rows[1][0];
+ rot[2] = HalfInverse * tmp.m_Rows[2][0];
+ }
+ else
+ {
+ // r22 is maximum diagonal term
+ rot[2] = 0.5f * sqrtf(tmp.m_Rows[2][2] - tmp.m_Rows[0][0] - tmp.m_Rows[1][1] + 1.0f);
+ HalfInverse = 0.5f / rot[2];
+ rot[0] = HalfInverse * tmp.m_Rows[2][0];
+ rot[1] = HalfInverse * tmp.m_Rows[2][1];
+ }
+ }
+ else
+ {
+ // r11 > r00
+ if (tmp.m_Rows[1][1] >= tmp.m_Rows[2][2])
+ {
+ // r11 is maximum diagonal term
+ rot[1] = 0.5f * sqrtf(tmp.m_Rows[1][1] - tmp.m_Rows[0][0] - tmp.m_Rows[2][2] + 1.0f);
+ HalfInverse = 0.5f / rot[1];
+ rot[0] = HalfInverse * tmp.m_Rows[1][0];
+ rot[2] = HalfInverse * tmp.m_Rows[2][1];
+ }
+ else
+ {
+ // r22 is maximum diagonal term
+ rot[2] = 0.5f * sqrtf(tmp.m_Rows[2][2] - tmp.m_Rows[0][0] - tmp.m_Rows[1][1] + 1.0f);
+ HalfInverse = 0.5f / rot[2];
+ rot[0] = HalfInverse * tmp.m_Rows[2][0];
+ rot[1] = HalfInverse * tmp.m_Rows[2][1];
+ }
+ }
+ }
+ }
+ else
+ {
+ // The angle is 0 and the matrix is the identity.
+ rot[0] = 0.0f;
+ rot[1] = 0.0f;
+ rot[2] = 1.0f;
+ }
+
+ return rot;
+ }
+
protected:
Vector4 m_Rows[4];
};