106 lines
3.4 KiB
C++
106 lines
3.4 KiB
C++
#pragma once
|
|
|
|
/// Template Parameterized (Generic) Matrix Functions.
|
|
|
|
#include <J3ML/LinearAlgebra/Quaternion.h>
|
|
|
|
|
|
|
|
namespace J3ML::LinearAlgebra {
|
|
|
|
/** Sets the top-left 3x3 area of the matrix to the rotation matrix about the X-axis. Elements
|
|
outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
|
|
in the order M*v, and clockwise if rotated in the order v*M.
|
|
@param m The matrix to store the result.
|
|
@param angle the rotation angle in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateX(Matrix &m, float angle) {
|
|
float sinz, cosz;
|
|
sinz = std::sin(angle);
|
|
cosz = std::cos(angle);
|
|
|
|
m[0][0] = 1.f;
|
|
m[0][1] = 0.f;
|
|
m[0][2] = 0.f;
|
|
m[1][0] = 0.f;
|
|
m[1][1] = cosz;
|
|
m[1][2] = -sinz;
|
|
m[2][0] = 0.f;
|
|
m[2][1] = sinz;
|
|
m[2][2] = cosz;
|
|
}
|
|
|
|
/** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Y-axis. Elements
|
|
outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
|
|
in the order M*v, and clockwise if rotated in the order v*M.
|
|
@param m The matrix to store the result
|
|
@param angle The rotation angle in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateY(Matrix &m, float angle) {
|
|
float sinz, cosz;
|
|
sinz = std::sin(angle);
|
|
cosz = std::cos(angle);
|
|
|
|
m[0][0] = cosz;
|
|
m[0][1] = 0.f;
|
|
m[0][2] = sinz;
|
|
m[1][0] = 0.f;
|
|
m[1][1] = 1.f;
|
|
m[1][2] = 0.f;
|
|
m[2][0] = -sinz;
|
|
m[2][1] = 0.f;
|
|
m[2][2] = cosz;
|
|
}
|
|
|
|
/** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
|
|
outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
|
|
in the order of M*v, and clockwise if rotated in the order v*M.
|
|
@param m The matrix to store the result.
|
|
@param angle The rotation angle in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateZ(Matrix &m, float angle) {
|
|
float sinz, cosz;
|
|
sinz = std::sin(angle);
|
|
cosz = std::cos(angle);
|
|
|
|
m[0][0] = cosz;
|
|
m[0][1] = -sinz;
|
|
m[0][2] = 0.f;
|
|
m[1][0] = sinz;
|
|
m[1][1] = cosz;
|
|
m[1][2] = 0.f;
|
|
m[2][0] = 0.f;
|
|
m[2][1] = 0.f;
|
|
m[2][2] = 1.f;
|
|
}
|
|
|
|
|
|
/** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
|
|
about the axis +d, rotating counterclockwise.
|
|
This function was adapted from https://www.geometrictools.com/Documentation/EulerAngles.pdf .
|
|
Parameters x y and z are the angles of rotation, in radians. */
|
|
template<typename Matrix>
|
|
void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z) {
|
|
// TODO: vectorize to compute 4 sines + cosines at one time;
|
|
float cx = std::cos(x);
|
|
float sx = std::sin(x);
|
|
float cy = std::cos(y);
|
|
float sy = std::sin(y);
|
|
float cz = std::cos(z);
|
|
float sz = std::sin(z);
|
|
|
|
m[0][0] = cy * cz;
|
|
m[0][1] = -cy * sz;
|
|
m[0][2] = sy;
|
|
m[1][0] = cz * sx * sy + cx * sz;
|
|
m[1][1] = cx * cz - sx * sy * sz;
|
|
m[1][2] = -cy * sx;
|
|
m[2][0] = -cx * cz * sy + sx * sz;
|
|
m[2][1] = cz * sx + cx * sy * sz;
|
|
m[2][2] = cx * cy;
|
|
}
|
|
|
|
|
|
|
|
}
|