mirror of https://github.com/CGAL/cgal
68 lines
2.0 KiB
C++
68 lines
2.0 KiB
C++
#include <CGAL/PDB/Quaternion.h>
|
|
CGAL_PDB_BEGIN_NAMESPACE
|
|
|
|
Quaternion::Quaternion(const double m[3][3]){
|
|
int i;
|
|
NT qs2, qx2, qy2, qz2, tmp;
|
|
NT norm;
|
|
|
|
for (i=0; i<4; ++i){
|
|
q_[i] = 0;
|
|
}
|
|
|
|
//first compute squared magnitudes of q components - at least one
|
|
//will be greater than 0 since q is unit magnitude
|
|
|
|
qs2 = 0.25 * (m[0][0] + m[1][1]
|
|
+ m[2][2] + 1);
|
|
qx2 = qs2 - 0.5 * (m[1][1] + m[2][2]);
|
|
qy2 = qs2 - 0.5 * (m[2][2] + m[0][0]);
|
|
qz2 = qs2 - 0.5 * (m[0][0] + m[1][1]);
|
|
|
|
//compute signed q components using numerically stable method
|
|
if ((qs2 >= qx2) & (qs2 >= qy2) & (qs2 >= qz2)){
|
|
q_[0] = sqrt(qs2);
|
|
tmp = 0.25 / q_[0];
|
|
q_[1] = (m[2][1] - m[1][2]) * tmp;
|
|
q_[2] = (m[0][2] - m[2][0]) * tmp;
|
|
q_[3] = (m[1][0] - m[0][1]) * tmp;
|
|
} else if ((qx2 >= qs2) & (qx2 >= qy2) & (qx2 >= qz2)){
|
|
q_[1] = sqrt(qx2);
|
|
tmp = 0.25 / q_[1];
|
|
q_[0] = (m[2][1] - m[1][2]) * tmp;
|
|
q_[2] = (m[0][1] + m[1][0]) * tmp;
|
|
q_[3] = (m[0][2] + m[2][0]) * tmp;
|
|
} else if ((qy2 >= qs2) & (qy2 >= qx2) & (qy2 >= qz2)){
|
|
q_[2] = sqrt(qy2);
|
|
tmp = 0.25 / q_[2];
|
|
q_[0] = (m[0][2] - m[2][0]) * tmp;
|
|
q_[3] = (m[1][2] + m[2][1]) * tmp;
|
|
q_[1] = (m[1][0] + m[0][1]) * tmp;
|
|
} else if ((qz2 >= qs2) & (qz2 >= qx2) & (qz2 >= qy2)){
|
|
q_[3] = sqrt(qz2);
|
|
tmp = 0.25 / q_[3];
|
|
q_[0] = (m[1][0] - m[0][1]) * tmp;
|
|
q_[1] = (m[2][0] + m[0][2]) * tmp;
|
|
q_[2] = (m[2][1] + m[1][2]) * tmp;
|
|
}
|
|
|
|
// for consistency, force positive scalar component [ (s; v) = (-s; -v) ]
|
|
if (q_[0] < 0){
|
|
for (i=0; i<4; ++i){
|
|
q_[i] = -q_[i];
|
|
}
|
|
}
|
|
|
|
norm = sqrt(q_[0]*q_[0] + q_[1]*q_[1]
|
|
+ q_[2]*q_[2] + q_[3]*q_[3]);
|
|
for (i=0; i<4; ++i){
|
|
q_[i] /= norm;
|
|
}
|
|
|
|
//the first element we can uniquely derive from others
|
|
// part_quarternion = Vector3D(quaternion[1], quaternion[2], quaternion[3]);
|
|
//for (i=0; i<3; ++i){
|
|
//part_quarternion[i] =
|
|
}
|
|
CGAL_PDB_END_NAMESPACE
|