Trafo rot (const Vector3D& A, float theta)
{
Trafo D;
float v, w, c = cos(theta), s = sin(theta), c1 = 1-c;
// compute unit vector E in direction of axis A
float n = Norm(A);
if (n == 0) return Trafo(0); // return 0 if axis is not defined
Vector3D E (A.x/n,A.y/n,A.z/n);
// no translation (last column is zero)
D.m[0][3] = D.m[1][3] = D.m[2][3] = 0;
// diagonal
D.m[0][0] = c + c1 * E.x * E.x;
D.m[1][1] = c + c1 * E.y * E.y;
D.m[2][2] = c + c1 * E.z * E.z;
// off diagonal
v = c1 * E.x * E.y; w = s * E.z;
D.m[0][1] = v + w;
D.m[1][0] = v - w;
v = c1 * E.x * E.z; w = -s * E.y;
D.m[0][2] = v + w;
D.m[2][0] = v - w;
v = c1 * E.y * E.z; w = s * E.x;
D.m[1][2] = v + w;
D.m[2][1] = v - w;
return D;
}
Trafo invert (const Trafo &A)
{
double d,d1,d2,d3,d10,d11,d12;
Trafo Ai;
d1 = A.m[1][1]*A.m[2][2]-A.m[1][2]*A.m[2][1];
d2 = A.m[1][0]*A.m[2][2]-A.m[1][2]*A.m[2][0];
d3 = A.m[1][0]*A.m[2][1]-A.m[1][1]*A.m[2][0];
d = A.m[0][0]*d1-A.m[0][1]*d2+A.m[0][2]*d3;
if (fabs(d) < 1e-6) return Trafo(0);
d10 = A.m[1][2]*A.m[2][3]-A.m[2][2]*A.m[1][3];
d11 = A.m[1][1]*A.m[2][3]-A.m[2][1]*A.m[1][3];
d12 = A.m[1][0]*A.m[2][3]-A.m[2][0]*A.m[1][3];
Ai.m[0][0] = d1/d;
Ai.m[1][0] = -d2/d;
Ai.m[2][0] = d3/d;
Ai.m[0][1] = (A.m[0][2]*A.m[2][1]-A.m[0][1]*A.m[2][2])/d;
Ai.m[1][1] = (A.m[0][0]*A.m[2][2]-A.m[0][2]*A.m[2][0])/d;
Ai.m[2][1] = (A.m[0][1]*A.m[2][0]-A.m[0][0]*A.m[2][1])/d;
Ai.m[0][2] = (A.m[0][1]*A.m[1][2]-A.m[0][2]*A.m[1][1])/d;
Ai.m[1][2] = (A.m[0][2]*A.m[1][0]-A.m[0][0]*A.m[1][2])/d;
Ai.m[2][2] = (A.m[0][0]*A.m[1][1]-A.m[1][0]*A.m[0][1])/d;
Ai.m[0][3] = (-A.m[0][1]*d10+A.m[0][2]*d11-A.m[0][3]*d1)/d;
Ai.m[1][3] = (A.m[0][0]*d10-A.m[0][2]*d12+A.m[0][3]*d2)/d;
Ai.m[2][3] = (-A.m[0][0]*d11+A.m[0][1]*d12-A.m[0][3]*d3)/d;
return Ai;
}
// interface layer to inherited class Mp3dView:
// set selected view for active graph
void GetSelectionView (Trafo& trafo, float& fov)
{
Mp3dView::GetTrafo(trafo,fov);
// undo translation of center
Trafo invT = invert(view->GetActiveGraphPtr()->viewcenter);
if (invT != Trafo(0)) trafo = trafo * invT;
}
/**
* @brief Forward transform
*
* @param m To transform
* @return Transform
*/
inline
Matrix <T>
operator* (const Matrix <T> & m) {
if (_wl_fam == ID)
return m;
else
{
Matrix <T> res (m);
Trafo (m, res);
return res;
}
}
/**
* @brief Forward transform
*
* @param m To transform
* @return Transform
*/
template <class T> Matrix<T>
operator* (const Matrix<T>& m) {
return Trafo (m);
}
/**
* @brief Forward transform
*
* @param m To transform
* @return Transform
*/
virtual Matrix< std::complex<T> >
operator* (const Matrix< std::complex<T> >& m) const {
return Trafo(m);
}
/**
* @brief Forward transform
*
* @param m To transform
* @return Transform
*/
inline virtual Matrix<CT>
operator* (const Matrix<CT>& m) const {
return Trafo(m);
}
virtual Matrix<T> operator* (const MatrixType<T>& m) const {
return Trafo(m);
}