mirror of https://github.com/CGAL/cgal
109 lines
3.1 KiB
C++
109 lines
3.1 KiB
C++
#include <CGAL/PDB/Quaternion.h>
|
|
#include <CGAL/PDB/Transform.h>
|
|
CGAL_PDB_BEGIN_NAMESPACE
|
|
|
|
/*Quaternion::Quaternion(const double m[3][3]){
|
|
|
|
}*/
|
|
|
|
Quaternion::Quaternion(Transform t) {
|
|
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 * (t.m(0,0) + t.m(1,1)
|
|
+ t.m(2,2) + 1);
|
|
qx2 = qs2 - 0.5 * (t.m(1,1) + t.m(2,2));
|
|
qy2 = qs2 - 0.5 * (t.m(2,2) + t.m(0,0));
|
|
qz2 = qs2 - 0.5 * (t.m(0,0) + t.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] = (t.m(2,1) - t.m(1,2)) * tmp;
|
|
q_[2] = (t.m(0,2) - t.m(2,0)) * tmp;
|
|
q_[3] = (t.m(1,0) - t.m(0,1)) * tmp;
|
|
} else if ((qx2 >= qs2) & (qx2 >= qy2) & (qx2 >= qz2)){
|
|
q_[1] = sqrt(qx2);
|
|
tmp = 0.25 / q_[1];
|
|
q_[0] = (t.m(2,1) - t.m(1,2)) * tmp;
|
|
q_[2] = (t.m(0,1) + t.m(1,0)) * tmp;
|
|
q_[3] = (t.m(0,2) + t.m(2,0)) * tmp;
|
|
} else if ((qy2 >= qs2) & (qy2 >= qx2) & (qy2 >= qz2)){
|
|
q_[2] = sqrt(qy2);
|
|
tmp = 0.25 / q_[2];
|
|
q_[0] = (t.m(0,2) - t.m(2,0)) * tmp;
|
|
q_[3] = (t.m(1,2) + t.m(2,1)) * tmp;
|
|
q_[1] = (t.m(1,0) + t.m(0,1)) * tmp;
|
|
} else if ((qz2 >= qs2) & (qz2 >= qx2) & (qz2 >= qy2)){
|
|
q_[3] = sqrt(qz2);
|
|
tmp = 0.25 / q_[3];
|
|
q_[0] = (t.m(1,0) - t.m(0,1)) * tmp;
|
|
q_[1] = (t.m(2,0) + t.m(0,2)) * tmp;
|
|
q_[2] = (t.m(2,1) + t.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] =
|
|
}
|
|
|
|
Transform Quaternion::transform() const {
|
|
return transform(Vector(0,0,0));
|
|
}
|
|
|
|
Transform Quaternion::transform(Vector translation) const {
|
|
NT q00 = operator[](0)*operator[](0);
|
|
NT q11 = operator[](1)*operator[](1);
|
|
NT q22 = operator[](2)*operator[](2);
|
|
NT q33 = operator[](3)*operator[](3);
|
|
NT q03 = operator[](0)*operator[](3);
|
|
NT q13 = operator[](1)*operator[](3);
|
|
NT q23 = operator[](2)*operator[](3);
|
|
NT q02 = operator[](0)*operator[](2);
|
|
NT q12 = operator[](1)*operator[](2);
|
|
NT q01 = operator[](0)*operator[](1);
|
|
|
|
//TNT::Matrix<NT> m(4,4);
|
|
NT rot[3][3];
|
|
rot[0][0] = q00 + q11 - q22 - q33;
|
|
rot[1][1] = q00 - q11 + q22 - q33;
|
|
rot[2][2] = q00 - q11 - q22 + q33;
|
|
rot[0][1] = 2.0*(q12-q03);
|
|
rot[1][0] = 2.0*(q12+q03);
|
|
rot[0][2] = 2.0*(q13+q02);
|
|
rot[2][0] = 2.0*(q13-q02);
|
|
rot[1][2] = 2.0*(q23-q01);
|
|
rot[2][1] = 2.0*(q23+q01);
|
|
|
|
Transform ret(rot[0][0], rot[0][1], rot[0][2],translation[0],
|
|
rot[1][0], rot[1][1], rot[1][2],translation[1],
|
|
rot[2][0], rot[2][1], rot[2][2],translation[2]);
|
|
|
|
return ret;
|
|
}
|
|
|
|
CGAL_PDB_END_NAMESPACE
|