libsegl/rotationsmatrix.cpp

#include "rotationsmatrix.h"

Rotationsmatrix::Rotationsmatrix() : Matrix(3,3) {
	set(x_axis, 0.0f);
}

Rotationsmatrix::Rotationsmatrix(axis _ax, float _deg) : Matrix(3,3) {
	set(_ax, _deg);
}

Rotationsmatrix::Rotationsmatrix(Punkt3D _rotvec, float _deg) : Matrix(3,3) {
	set(_rotvec, _deg);	
}

void Rotationsmatrix::set(Punkt3D _rotvec, float _deg) {
	rotvec = _rotvec;
	rotvec.normalize();
	deg = _deg;
	initRot();
}

void Rotationsmatrix::set(axis _ax, float _deg) {
	deg = _deg;
	
	switch(_ax) {
		case x_axis:
			rotvec.set(1.0f, 0.0f, 0.0f);
		break;
		case y_axis:
			rotvec.set(0.0f, 1.0f, 0.0f);
		break;
		case z_axis:
			rotvec.set(0.0f, 0.0f, 1.0f);
		break;	
	}
	initRot();
}

void Rotationsmatrix::set(float _deg) {
	deg = _deg;
// 	rotvec.print();
	initRot();
}

void Rotationsmatrix::initRot() {
	c[0][0] = scos(deg) + pow(rotvec.x,2.0f) * (1 - scos(deg));
	c[0][1] = rotvec.x * rotvec.y * (1 - scos(deg)) - rotvec.z * ssin(deg);
	c[0][2] = rotvec.x * rotvec.z * (1 - scos(deg)) + rotvec.y * ssin(deg);

	c[1][0] = rotvec.y * rotvec.x * (1 - scos(deg)) + rotvec.z * ssin(deg);
	c[1][1] = scos(deg) + pow(rotvec.y,2.0f) * (1 - scos(deg));
	c[1][2] = rotvec.y * rotvec.z * (1 - scos(deg)) - rotvec.x * ssin(deg);

	c[2][0] = rotvec.z * rotvec.x * (1 - scos(deg)) - rotvec.y * ssin(deg);
	c[2][1] = rotvec.z * rotvec.y * (1 - scos(deg)) + rotvec.x * ssin(deg);
	c[2][2] = scos(deg) + pow(rotvec.z,2.0f) * (1 - scos(deg));
}

Punkt3D Rotationsmatrix::operator*(const Punkt3D &p) {
	Matrix erg(3,1);
	Punkt3D erg2;

	erg =  (((Matrix*)(this))->operator*(Matrix(p)));

	const float **mat = erg.getMatrix();
	erg2.set(mat[0][0], mat[1][0], mat[2][0]);

	return erg2;
}

Rotationsmatrix Rotationsmatrix::operator*(const Rotationsmatrix &p) {
	Matrix m(3, 3);
	Rotationsmatrix rotmat;
	m = ((Matrix)(*this) * (Matrix)p);
	const float **mat = m.getMatrix();
	rotmat.c[0][0] = mat[0][0];
	rotmat.c[0][1] = mat[0][1];
	rotmat.c[0][2] = mat[0][2];
	rotmat.c[1][0] = mat[1][0];
	rotmat.c[1][1] = mat[1][1];
	rotmat.c[1][2] = mat[1][2];
	rotmat.c[2][0] = mat[2][0];
	rotmat.c[2][1] = mat[2][1];
	rotmat.c[2][2] = mat[2][2];

	return rotmat;
}


Rotationsmatrix::~Rotationsmatrix() {

}