#include "Lib3ds_matrix.h"
#include <math.h>
Lib3ds_matrix::Lib3ds_matrix()
{
}
/*!
* Clear a matrix to all zeros.
*
* \param m Matrix to be cleared.
*/
void Lib3ds_matrix::lib3ds_matrix_zero(float m[4][4])
{
int i, j;
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) m[i][j] = 0.0f;
}
}
/*!
* Set a matrix to identity.
*
* \param m Matrix to be set.
*/
void Lib3ds_matrix::lib3ds_matrix_identity(float m[4][4])
{
int i, j;
for (i = 0; i < 4; i++) {
for (j = 0; j < 4; j++) m[i][j] = 0.0;
}
for (i = 0; i < 4; i++) m[i][i] = 1.0;
}
/*!
* Copy a matrix.
*/
void Lib3ds_matrix::lib3ds_matrix_copy(float dest[4][4], float src[4][4])
{
memcpy(dest, src, 16 * sizeof(float));
}
/*!
* Negate a matrix -- all elements negated.
*/
void Lib3ds_matrix::lib3ds_matrix_neg(float m[4][4])
{
int i, j;
for (j = 0; j < 4; j++) {
for (i = 0; i < 4; i++) {
m[j][i] = -m[j][i];
}
}
}
/*!
* Transpose a matrix in place.
*/
void Lib3ds_matrix::lib3ds_matrix_transpose(float m[4][4])
{
int i, j;
float swp;
for (j = 0; j < 4; j++) {
for (i = j + 1; i < 4; i++) {
swp = m[j][i];
m[j][i] = m[i][j];
m[i][j] = swp;
}
}
}
/*!
* Add two matrices.
*/
void Lib3ds_matrix::lib3ds_matrix_add(float m[4][4], float a[4][4], float b[4][4])
{
int i, j;
for (j = 0; j < 4; j++) {
for (i = 0; i < 4; i++) {
m[j][i] = a[j][i] + b[j][i];
}
}
}
/*!
* Subtract two matrices.
*
* \param m Result.
* \param a Addend.
* \param b Minuend.
*/
void Lib3ds_matrix::lib3ds_matrix_sub(float m[4][4], float a[4][4], float b[4][4])
{
int i, j;
for (j = 0; j < 4; j++) {
for (i = 0; i < 4; i++) {
m[j][i] = a[j][i] - b[j][i];
}
}
}
/*!
* Multiplies a matrix by a second one (m = m * n).
*/
void Lib3ds_matrix::lib3ds_matrix_mult(float m[4][4], float a[4][4], float b[4][4])
{
float tmp[4][4];
int i, j, k;
float ab;
memcpy(tmp, a, 16 * sizeof(float));
for (j = 0; j < 4; j++) {
for (i = 0; i < 4; i++) {
ab = 0.0f;
for (k = 0; k < 4; k++) ab += tmp[k][i] * b[j][k];
m[j][i] = ab;
}
}
}
/*!
* Multiply a matrix by a scalar.
*
* \param m Matrix to be set.
* \param k Scalar.
*/
void Lib3ds_matrix::lib3ds_matrix_scalar(float m[4][4], float k)
{
int i, j;
for (j = 0; j < 4; j++) {
for (i = 0; i < 4; i++) {
m[j][i] *= k;
}
}
}
float Lib3ds_matrix::det2x2(
float a, float b,
float c, float d)
{
return((a)*(d) - (b)*(c));
}
float Lib3ds_matrix::det3x3(
float a1, float a2, float a3,
float b1, float b2, float b3,
float c1, float c2, float c3)
{
return(
a1*det2x2(b2, b3, c2, c3) -
b1*det2x2(a2, a3, c2, c3) +
c1*det2x2(a2, a3, b2, b3)
);
}
/*!
* Find determinant of a matrix.
*/
float Lib3ds_matrix::lib3ds_matrix_det(float m[4][4])
{
float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
a1 = m[0][0];
b1 = m[1][0];
c1 = m[2][0];
d1 = m[3][0];
a2 = m[0][1];
b2 = m[1][1];
c2 = m[2][1];
d2 = m[3][1];
a3 = m[0][2];
b3 = m[1][2];
c3 = m[2][2];
d3 = m[3][2];
a4 = m[0][3];
b4 = m[1][3];
c4 = m[2][3];
d4 = m[3][3];
return(
a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4) -
b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4) +
c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4) -
d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4)
);
}
/*!
* Invert a matrix in place.
*
* \param m Matrix to invert.
*
* \return LIB3DS_TRUE on success, LIB3DS_FALSE on failure.
*
* GGemsII, K.Wu, Fast Matrix Inversion
*/
int Lib3ds_matrix::lib3ds_matrix_inv(float m[4][4])
{
int i, j, k;
int pvt_i[4], pvt_j[4]; /* Locations of pivot elements */
float pvt_val; /* Value of current pivot element */
float hold; /* Temporary storage */
float determinat;
determinat = 1.0f;
for (k = 0; k < 4; k++) {
/* Locate k'th pivot element */
pvt_val = m[k][k]; /* Initialize for search */
pvt_i[k] = k;
pvt_j[k] = k;
for (i = k; i < 4; i++) {
for (j = k; j < 4; j++) {
if (fabs(m[i][j]) > fabs(pvt_val)) {
pvt_i[k] = i;
pvt_j[k] = j;
pvt_val = m[i][j];
}
}
}
/* Product of pivots, gives determinant when finished */
determinat *= pvt_val;
if (fabs(determinat) < LIB3DS_EPSILON) {
return(FALSE); /* Matrix is singular (zero determinant) */
}
/* "Interchange" rows (with sign change stuff) */
i = pvt_i[k];
if (i != k) { /* If rows are different */
for (j = 0; j < 4; j++) {
hold = -m[k][j];
m[k][j] = m[i][j];
m[i][j] = hold;
}
}
/* "Interchange" columns */
j = pvt_j[k];
if (j != k) { /* If columns are different */
for (i = 0; i < 4; i++) {
hold = -m[i][k];
m[i][k] = m[i][j];
m[i][j] = hold;
}
}
/* Divide column by minus pivot value */
for (i = 0; i < 4; i++) {
if (i != k) m[i][k] /= (-pvt_val) ;
}
/* Reduce the matrix */
for (i = 0; i < 4; i++) {
hold = m[i][k];
for (j = 0; j < 4; j++) {
if (i != k && j != k) m[i][j] += hold * m[k][j];
}
}
/* Divide row by pivot */
for (j = 0; j < 4; j++) {
if (j != k) m[k][j] /= pvt_val;
}
/* Replace pivot by reciprocal (at last we can touch it). */
m[k][k] = 1.0f / pvt_val;
}
/* That was most of the work, one final pass of row/column interchange */
/* to finish */
for (k = 4 - 2; k >= 0; k--) { /* Don't need to work with 1 by 1 corner*/
i = pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */
if (i != k) { /* If rows are different */
for (j = 0; j < 4; j++) {
hold = m[k][j];
m[k][j] = -m[i][j];
m[i][j] = hold;
}
}
j = pvt_i[k]; /* Columns to swap correspond to pivot ROW */
if (j != k) /* If columns are different */
for (i = 0; i < 4; i++) {
hold = m[i][k];
m[i][k] = -m[i][j];
m[i][j] = hold;
}
}
return(TRUE);
}
/*!
* Apply a translation to a matrix.
*/
void Lib3ds_matrix::lib3ds_matrix_translate(float m[4][4], float x, float y, float z)
{
int i;
for (i = 0; i < 3; i++) {
m[3][i] += m[0][i] * x + m[1][i] * y + m[2][i] * z;
}
}
/*!
* Apply scale factors to a matrix.
*/
void Lib3ds_matrix::lib3ds_matrix_scale(float m[4][4], float x, float y, float z)
{
int i;
for (i = 0; i < 4; i++) {
m[0][i] *= x;
m[1][i] *= y;
m[2][i] *= z;
}
}
/*!
* Apply a rotation about an arbitrary axis to a matrix.
*/
void Lib3ds_matrix::lib3ds_matrix_rotate_quat(float m[4][4], float q[4])
{
float s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, l;
float R[4][4];
l = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
if (fabs(l) < LIB3DS_EPSILON) {
s = 1.0f;
} else {
s = 2.0f / l;
}
xs = q[0] * s;
ys = q[1] * s;
zs = q[2] * s;
wx = q[3] * xs;
wy = q[3] * ys;
wz = q[3] * zs;
xx = q[0] * xs;
xy = q[0] * ys;
xz = q[0] * zs;
yy = q[1] * ys;
yz = q[1] * zs;
zz = q[2] * zs;
R[0][0] = 1.0f - (yy + zz);
R[1][0] = xy - wz;
R[2][0] = xz + wy;
R[0][1] = xy + wz;
R[1][1] = 1.0f - (xx + zz);
R[2][1] = yz - wx;
R[0][2] = xz - wy;
R[1][2] = yz + wx;
R[2][2] = 1.0f - (xx + yy);
R[3][0] = R[3][1] = R[3][2] = R[0][3] = R[1][3] = R[2][3] = 0.0f;
R[3][3] = 1.0f;
lib3ds_matrix_mult(m, m, R);
}
/*!
* Apply a rotation about an arbitrary axis to a matrix.
*/
void Lib3ds_matrix::lib3ds_matrix_rotate(float m[4][4], float angle, float ax, float ay, float az)
{
float q[4];
float axis[3];
lib3ds_vector_make(axis, ax, ay, az);
lib3ds_quat_axis_angle(q, axis, angle);
lib3ds_matrix_rotate_quat(m, q);
}
/*!
* Compute a camera matrix based on position, target and roll.
*
* Generates a translate/rotate matrix that maps world coordinates
* to camera coordinates. Resulting matrix does not include perspective
* transform.
*
* \param matrix Destination matrix.
* \param pos Camera position
* \param tgt Camera target
* \param roll Roll angle
*/
void Lib3ds_matrix::lib3ds_matrix_camera(float matrix[4][4], float pos[3], float tgt[3], float roll)
{
float M[4][4];
float x[3], y[3], z[3];
lib3ds_vector_sub(y, tgt, pos);
lib3ds_vector_normalize(y);
if (y[0] != 0. || y[1] != 0) {
z[0] = 0;
z[1] = 0;
z[2] = 1.0;
} else { /* Special case: looking straight up or down z axis */
z[0] = -1.0;
z[1] = 0;
z[2] = 0;
}
lib3ds_vector_cross(x, y, z);
lib3ds_vector_cross(z, x, y);
lib3ds_vector_normalize(x);
lib3ds_vector_normalize(z);
lib3ds_matrix_identity(M);
M[0][0] = x[0];
M[1][0] = x[1];
M[2][0] = x[2];
M[0][1] = y[0];
M[1][1] = y[1];
M[2][1] = y[2];
M[0][2] = z[0];
M[1][2] = z[1];
M[2][2] = z[2];
lib3ds_matrix_identity(matrix);
lib3ds_matrix_rotate(matrix, roll, 0, 1, 0);
lib3ds_matrix_mult(matrix, matrix, M);
lib3ds_matrix_translate(matrix, -pos[0], -pos[1], -pos[2]);
}
Submit an article or tip