void RotateVector(const VECTOR3 &Initial, const VECTOR3 &Angles, VECTOR3 &Result)
{
MATRIX3 RotMatrixX, RotMatrixY, RotMatrixZ;
VECTOR3 AfterZ, AfterZY; // Temporary variables
GetRotMatrixX(Angles.x, RotMatrixX);
GetRotMatrixY(Angles.y, RotMatrixY);
GetRotMatrixZ(Angles.z, RotMatrixZ);
MultiplyByMatrix(Initial, RotMatrixZ, AfterZ);
MultiplyByMatrix(AfterZ, RotMatrixY, AfterZY);
MultiplyByMatrix(AfterZY, RotMatrixX, Result);
}
void Polygon::RotateArbitrary(float angle, Vector3 pointa, Vector3 pointb)
{
//Setup values
Vector3 v;
v = pointa - pointb;
v = v.Normalized();
float d = sqrt(pow(v.y, 2) + pow(v.z, 2));
//Step 1
Translate(-pointa);
//Step 2
Matrix<4, 4> a = {1, 0, 0, 0,
0, v.z/d, -v.y/d, 0,
0, v.y/d, v.z/d, 0,
0, 0, 0, 1};
Matrix<4, 4> b = {d, 0, -v.x, 0,
0, 1, 0, 0,
v.x, 0, d, 0,
0, 0, 0, 1};
for(int i=0; (unsigned) i<vertices.size(); i++)
{
vertices[i] = MultiplyByMatrix(vertices[i], a);
vertices[i] = MultiplyByMatrix(vertices[i], b);
}
//Step 3
RotateZAxis(angle);
//Step 4
Matrix<4, 4> c = {1, 0, 0, 0,
0, v.z/d, v.y/d, 0,
0, -v.y/d, v.z/d, 0,
0, 0, 0, 1};
Matrix<4, 4> e = {d, 0, v.x, 0,
0, 1, 0, 0,
-v.x, 0, d, 0,
0, 0, 0, 1};
for(int i=0; (unsigned) i<vertices.size(); i++)
{
vertices[i] = MultiplyByMatrix(vertices[i], e);
vertices[i] = MultiplyByMatrix(vertices[i], c);
}
//Step 5
Translate(pointa);
}
void Polygon::RotateXAxis(float angle)
{
angle *= 3.14159/180;
for(int i=0; (unsigned) i<vertices.size(); i++)
{
Matrix<4, 4> a = {1, 0, 0, 0,
0, cos(angle), -sin(angle), 0,
0, sin(angle), cos(angle), 0,
0, 0, 0, 1};
vertices[i] = MultiplyByMatrix(vertices[i], a);
}
}
void Polygon::Translate(Vector3 translate_amount)
{
if(vertices.empty()) return;
for(int i=0; (unsigned) i<vertices.size(); i++)
{
Matrix<4, 4> a = {1, 0, 0, translate_amount.x,
0, 1, 0, translate_amount.y,
0, 0, 1, translate_amount.z,
0, 0, 0, 1};
vertices[i] = MultiplyByMatrix(vertices[i], a);
}
}
Vector Vector::Rotate3DZ(double a_alfa)
{
double matrix[3][4];
matrix[0][0] = cos(a_alfa);
matrix[0][1] = -sin(a_alfa);
matrix[0][2] = 0;
matrix[0][3] = 0;
matrix[1][0] = sin(a_alfa);
matrix[1][1] = cos(a_alfa);
matrix[1][2] = 0;
matrix[1][3] = 0;
matrix[2][0] = 0;
matrix[2][1] = 0;
matrix[2][2] = 1;
matrix[2][3] = 0;
return MultiplyByMatrix(matrix);
}