//=======================================================================
|
//=======================================================================
|
// Project Monophony
|
// Project Monophony
|
// Wire-Frame 3D Graphics Accelerator IP Core
|
// Wire-Frame 3D Graphics Accelerator IP Core
|
//
|
//
|
// File:
|
// File:
|
// mp_matrix4.c
|
// mp_matrix4.c
|
//
|
//
|
// Abstract:
|
// Abstract:
|
// matrix4 class implementation
|
// matrix4 class implementation
|
//
|
//
|
// Author:
|
// Author:
|
// Kenji Ishimaru (info.wf3d@gmail.com)
|
// Kenji Ishimaru (info.info.wf3d@gmail.com)
|
//
|
//
|
//======================================================================
|
//======================================================================
|
//
|
//
|
// Copyright (c) 2015, Kenji Ishimaru
|
// Copyright (c) 2015, Kenji Ishimaru
|
// All rights reserved.
|
// All rights reserved.
|
//
|
//
|
// Redistribution and use in source and binary forms, with or without
|
// Redistribution and use in source and binary forms, with or without
|
// modification, are permitted provided that the following conditions are met:
|
// modification, are permitted provided that the following conditions are met:
|
//
|
//
|
// -Redistributions of source code must retain the above copyright notice,
|
// -Redistributions of source code must retain the above copyright notice,
|
// this list of conditions and the following disclaimer.
|
// this list of conditions and the following disclaimer.
|
// -Redistributions in binary form must reproduce the above copyright notice,
|
// -Redistributions in binary form must reproduce the above copyright notice,
|
// this list of conditions and the following disclaimer in the documentation
|
// this list of conditions and the following disclaimer in the documentation
|
// and/or other materials provided with the distribution.
|
// and/or other materials provided with the distribution.
|
//
|
//
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
|
// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
// THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
|
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
|
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
|
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
// EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
// EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
//
|
//
|
// Revision History
|
// Revision History
|
|
|
#include <math.h>
|
#include <math.h>
|
#include "mp_matrix4.h"
|
#include "mp_matrix4.h"
|
#include "mp_lib.h"
|
#include "mp_lib.h"
|
|
|
extern t_fui u_fui;
|
extern t_fui u_fui;
|
|
|
static int TR[16] = { 0,4,8,12,1,5,9,13,2,6,10,14,3,7,11,15 };
|
static int TR[16] = { 0,4,8,12,1,5,9,13,2,6,10,14,3,7,11,15 };
|
static int AA[16][16] = {
|
static int AA[16][16] = {
|
/* aa00 */ {5,6,7,9,10,11,13,14,15},
|
/* aa00 */ {5,6,7,9,10,11,13,14,15},
|
/* aa10 */ {1,2,3,9,10,11,13,14,15},
|
/* aa10 */ {1,2,3,9,10,11,13,14,15},
|
/* aa20 */ {1,2,3,5,6,7,13,14,15},
|
/* aa20 */ {1,2,3,5,6,7,13,14,15},
|
/* aa30 */ {1,2,3,5,6,7,9,10,11},
|
/* aa30 */ {1,2,3,5,6,7,9,10,11},
|
/* aa01 */ {4,6,7,8,10,11,12,14,15},
|
/* aa01 */ {4,6,7,8,10,11,12,14,15},
|
/* aa11 */ {0,2,3,8,10,11,12,14,15},
|
/* aa11 */ {0,2,3,8,10,11,12,14,15},
|
/* aa21 */ {0,2,3,4,6,7,12,14,15},
|
/* aa21 */ {0,2,3,4,6,7,12,14,15},
|
/* aa31 */ {0,2,3,4,6,7,8,10,11},
|
/* aa31 */ {0,2,3,4,6,7,8,10,11},
|
/* aa02 */ {4,5,7,8,9,11,12,13,15},
|
/* aa02 */ {4,5,7,8,9,11,12,13,15},
|
/* aa12 */ {0,1,3,8,9,11,12,13,15},
|
/* aa12 */ {0,1,3,8,9,11,12,13,15},
|
/* aa22 */ {0,1,3,4,5,7,12,13,15},
|
/* aa22 */ {0,1,3,4,5,7,12,13,15},
|
/* aa32 */ {0,1,3,4,5,7,8,9,11},
|
/* aa32 */ {0,1,3,4,5,7,8,9,11},
|
/* aa03 */ {4,5,6,8,9,10,12,13,14},
|
/* aa03 */ {4,5,6,8,9,10,12,13,14},
|
/* aa13 */ {0,1,2,8,9,10,12,13,14},
|
/* aa13 */ {0,1,2,8,9,10,12,13,14},
|
/* aa23 */ {0,1,2,4,5,6,12,13,14},
|
/* aa23 */ {0,1,2,4,5,6,12,13,14},
|
/* aa33 */ {0,1,2,4,5,6,8,9,10}};
|
/* aa33 */ {0,1,2,4,5,6,8,9,10}};
|
|
|
void mp_matrix4_init(mp_matrix4 *dst) {
|
void mp_matrix4_init(mp_matrix4 *dst) {
|
int i;
|
int i;
|
for (i=0;i<16;i++) dst->a[i] = 0.0;
|
for (i=0;i<16;i++) dst->a[i] = 0.0;
|
dst->a[0] = dst->a[5] = dst->a[10] = dst->a[15] = 1.0;
|
dst->a[0] = dst->a[5] = dst->a[10] = dst->a[15] = 1.0;
|
}
|
}
|
|
|
void mp_matrix4_set0(mp_matrix4 *dst, float b[]) {
|
void mp_matrix4_set0(mp_matrix4 *dst, float b[]) {
|
int i;
|
int i;
|
for (i=0;i<16;i++) dst->a[i] = b[i];
|
for (i=0;i<16;i++) dst->a[i] = b[i];
|
}
|
}
|
|
|
void mp_matrix4_set0_t(mp_matrix4 *dst, float b[]) {
|
void mp_matrix4_set0_t(mp_matrix4 *dst, float b[]) {
|
int i;
|
int i;
|
for (i=0;i<16;i++) dst->a[TR[i]] = b[i];
|
for (i=0;i<16;i++) dst->a[TR[i]] = b[i];
|
}
|
}
|
|
|
void mp_matrix4_set1(mp_matrix4 *dst, unsigned int b[]) {
|
void mp_matrix4_set1(mp_matrix4 *dst, unsigned int b[]) {
|
int i;
|
int i;
|
for (i = 0; i <16; i++) {
|
for (i = 0; i <16; i++) {
|
u_fui.ui= b[i];
|
u_fui.ui= b[i];
|
dst->a[i] = u_fui.f;
|
dst->a[i] = u_fui.f;
|
}
|
}
|
}
|
}
|
|
|
void mp_matrix4_set2(mp_matrix4 *dst, mp_matrix4 *t) {
|
void mp_matrix4_set2(mp_matrix4 *dst, mp_matrix4 *t) {
|
int i;
|
int i;
|
for (i=0;i<16;i++) dst->a[i] = t->a[i];
|
for (i=0;i<16;i++) dst->a[i] = t->a[i];
|
}
|
}
|
|
|
void mp_matrix4_identity(mp_matrix4 *dst) {
|
void mp_matrix4_identity(mp_matrix4 *dst) {
|
mp_matrix4_init(dst);
|
mp_matrix4_init(dst);
|
}
|
}
|
|
|
void mp_matrix4_assign(mp_matrix4 *dst, mp_matrix4 *v) {
|
void mp_matrix4_assign(mp_matrix4 *dst, mp_matrix4 *v) {
|
int i;
|
int i;
|
for (i=0 ; i < 16 ; i++ ) dst->a[i] = v->a[i];
|
for (i=0 ; i < 16 ; i++ ) dst->a[i] = v->a[i];
|
}
|
}
|
|
|
void mp_matrix4_multiply_vector4(mp_vector4 *dst, mp_matrix4 *m, mp_vector4 *v) {
|
void mp_matrix4_multiply_vector4(mp_vector4 *dst, mp_matrix4 *m, mp_vector4 *v) {
|
mp_vector4 t;
|
mp_vector4 t;
|
mp_vector4_set2(&t, v);
|
mp_vector4_set2(&t, v);
|
dst->x = m->a[0]*t.x + m->a[1]*t.y + m->a[2]*t.z + m->a[3]*t.w;
|
dst->x = m->a[0]*t.x + m->a[1]*t.y + m->a[2]*t.z + m->a[3]*t.w;
|
dst->y = m->a[4]*t.x + m->a[5]*t.y + m->a[6]*t.z + m->a[7]*t.w;
|
dst->y = m->a[4]*t.x + m->a[5]*t.y + m->a[6]*t.z + m->a[7]*t.w;
|
dst->z = m->a[8]*t.x + m->a[9]*t.y + m->a[10]*t.z + m->a[11]*t.w;
|
dst->z = m->a[8]*t.x + m->a[9]*t.y + m->a[10]*t.z + m->a[11]*t.w;
|
dst->w = m->a[12]*t.x + m->a[13]*t.y + m->a[14]*t.z + m->a[15]*t.w;
|
dst->w = m->a[12]*t.x + m->a[13]*t.y + m->a[14]*t.z + m->a[15]*t.w;
|
}
|
}
|
|
|
void mp_matrix4_multiply_f(mp_matrix4 *m, float *v) {
|
void mp_matrix4_multiply_f(mp_matrix4 *m, float *v) {
|
float r[4];
|
float r[4];
|
r[0] = m->a[0]*v[0] + m->a[1]*v[1] + m->a[2]*v[2] + m->a[3]*v[3];
|
r[0] = m->a[0]*v[0] + m->a[1]*v[1] + m->a[2]*v[2] + m->a[3]*v[3];
|
r[1] = m->a[4]*v[0] + m->a[5]*v[1] + m->a[6]*v[2] + m->a[7]*v[3];
|
r[1] = m->a[4]*v[0] + m->a[5]*v[1] + m->a[6]*v[2] + m->a[7]*v[3];
|
r[2] = m->a[8]*v[0] + m->a[9]*v[1] + m->a[10]*v[2] + m->a[11]*v[3];
|
r[2] = m->a[8]*v[0] + m->a[9]*v[1] + m->a[10]*v[2] + m->a[11]*v[3];
|
r[3] = m->a[12]*v[0] + m->a[13]*v[1] + m->a[14]*v[2] + m->a[15]*v[3];
|
r[3] = m->a[12]*v[0] + m->a[13]*v[1] + m->a[14]*v[2] + m->a[15]*v[3];
|
v[0] = r[0];
|
v[0] = r[0];
|
v[1] = r[1];
|
v[1] = r[1];
|
v[2] = r[2];
|
v[2] = r[2];
|
v[3] = r[3];
|
v[3] = r[3];
|
}
|
}
|
|
|
void mp_matrix4_multiply_f2(mp_matrix4 *m, float *dst, float *src) {
|
void mp_matrix4_multiply_f2(mp_matrix4 *m, float *dst, float *src) {
|
// w is always 1.0
|
// w is always 1.0
|
dst[0] = m->a[0]*src[0] + m->a[1]*src[1] + m->a[2]*src[2] + m->a[3];
|
dst[0] = m->a[0]*src[0] + m->a[1]*src[1] + m->a[2]*src[2] + m->a[3];
|
dst[1] = m->a[4]*src[0] + m->a[5]*src[1] + m->a[6]*src[2] + m->a[7];
|
dst[1] = m->a[4]*src[0] + m->a[5]*src[1] + m->a[6]*src[2] + m->a[7];
|
dst[2] = m->a[8]*src[0] + m->a[9]*src[1] + m->a[10]*src[2] + m->a[11];
|
dst[2] = m->a[8]*src[0] + m->a[9]*src[1] + m->a[10]*src[2] + m->a[11];
|
dst[3] = m->a[12]*src[0] + m->a[13]*src[1] + m->a[14]*src[2] + m->a[15];
|
dst[3] = m->a[12]*src[0] + m->a[13]*src[1] + m->a[14]*src[2] + m->a[15];
|
}
|
}
|
|
|
void mp_matrix4_multiply_matrix4(mp_matrix4 *dst, mp_matrix4 *mm, mp_matrix4 *b) {
|
void mp_matrix4_multiply_matrix4(mp_matrix4 *dst, mp_matrix4 *mm, mp_matrix4 *b) {
|
int i,j,k,m,n,p,r,s;
|
int i,j,k,m,n,p,r,s;
|
mp_matrix4 c;
|
mp_matrix4 c;
|
for ( i = r = 0 ; i < 4 ; i ++, r += 4 ) {
|
for ( i = r = 0 ; i < 4 ; i ++, r += 4 ) {
|
for ( j = 0 ; j < 4 ; j ++ ) {
|
for ( j = 0 ; j < 4 ; j ++ ) {
|
m = r + j;
|
m = r + j;
|
c.a[m] = 0.0;
|
c.a[m] = 0.0;
|
for ( k = s = 0 ; k < 4 ; k++, s += 4 ){
|
for ( k = s = 0 ; k < 4 ; k++, s += 4 ){
|
n = r + k;
|
n = r + k;
|
p = s + j;
|
p = s + j;
|
c.a[m] += b->a[n] * mm->a[p];
|
c.a[m] += b->a[n] * mm->a[p];
|
}
|
}
|
}
|
}
|
}
|
}
|
for ( k = 0 ; k < 16 ; k++ ) dst->a[k] = c.a[k];
|
for ( k = 0 ; k < 16 ; k++ ) dst->a[k] = c.a[k];
|
}
|
}
|
|
|
void mp_matrix4_multiply_matrix4i(mp_matrix4 *dst, mp_matrix4 *mm, mp_matrix4 *b) {
|
void mp_matrix4_multiply_matrix4i(mp_matrix4 *dst, mp_matrix4 *mm, mp_matrix4 *b) {
|
int i,j,k,m,n,p,r,s;
|
int i,j,k,m,n,p,r,s;
|
mp_matrix4 c;
|
mp_matrix4 c;
|
for ( i = r = 0 ; i < 4 ; i ++, r += 4 ) {
|
for ( i = r = 0 ; i < 4 ; i ++, r += 4 ) {
|
for ( j = 0 ; j < 4 ; j ++ ) {
|
for ( j = 0 ; j < 4 ; j ++ ) {
|
m = r + j;
|
m = r + j;
|
c.a[m] = 0.0;
|
c.a[m] = 0.0;
|
for ( k = s = 0 ; k < 4 ; k++, s += 4 ){
|
for ( k = s = 0 ; k < 4 ; k++, s += 4 ){
|
n = r + k;
|
n = r + k;
|
p = s + j;
|
p = s + j;
|
c.a[m] += mm->a[n] * b->a[p];
|
c.a[m] += mm->a[n] * b->a[p];
|
}
|
}
|
}
|
}
|
}
|
}
|
for ( k = 0 ; k < 16 ; k++ ) dst->a[k] = c.a[k];
|
for ( k = 0 ; k < 16 ; k++ ) dst->a[k] = c.a[k];
|
}
|
}
|
|
|
void mp_matrix4_transpose(mp_matrix4 *dst,mp_matrix4 *src) {
|
void mp_matrix4_transpose(mp_matrix4 *dst,mp_matrix4 *src) {
|
int i;
|
int i;
|
for (i = 0 ; i < 16 ; i++ ){
|
for (i = 0 ; i < 16 ; i++ ){
|
dst->a[i] = src->a[TR[i]];
|
dst->a[i] = src->a[TR[i]];
|
}
|
}
|
}
|
}
|
|
|
void mp_matrix4_inverse(mp_matrix4 *dst,mp_matrix4 *src) {
|
void mp_matrix4_inverse(mp_matrix4 *dst,mp_matrix4 *src) {
|
float determinant4, determinant3;
|
float determinant4, determinant3;
|
float v[9];
|
float v[9];
|
|
|
int m,n,sign;
|
int m,n,sign;
|
int i,j;
|
int i,j;
|
mp_matrix3 w;
|
mp_matrix3 w;
|
for (i=0 ; i < 16 ; i++ ){
|
for (i=0 ; i < 16 ; i++ ){
|
m = (i+1)/4 + 1; n = (i+1)%4;
|
m = (i+1)/4 + 1; n = (i+1)%4;
|
if (n==0) { n = 4; m--; }
|
if (n==0) { n = 4; m--; }
|
if ((m+n)%2 == 1) sign = -1; else sign = 1;
|
if ((m+n)%2 == 1) sign = -1; else sign = 1;
|
for (j=0 ; j < 9 ; j++ ) v[j] = src->a[AA[i][j]];
|
for (j=0 ; j < 9 ; j++ ) v[j] = src->a[AA[i][j]];
|
mp_matrix3_set0(&w, v);
|
mp_matrix3_set0(&w, v);
|
determinant3 = mp_matrix3_determinant0(&w);
|
determinant3 = mp_matrix3_determinant0(&w);
|
dst->a[i] = sign * determinant3;
|
dst->a[i] = sign * determinant3;
|
}
|
}
|
determinant4 =
|
determinant4 =
|
src->a[0]*dst->a[0]
|
src->a[0]*dst->a[0]
|
+ src->a[4]*dst->a[1]
|
+ src->a[4]*dst->a[1]
|
+ src->a[8]*dst->a[2]
|
+ src->a[8]*dst->a[2]
|
+ src->a[12]*dst->a[3];
|
+ src->a[12]*dst->a[3];
|
|
|
determinant4 = 1.0F / determinant4;
|
determinant4 = 1.0F / determinant4;
|
for (i=0 ; i < 16 ; i++ ) dst->a[i] *= determinant4;
|
for (i=0 ; i < 16 ; i++ ) dst->a[i] *= determinant4;
|
}
|
}
|
|
|
void mp_matrix4_rotate(mp_matrix4 *dst, mp_vector3 *v, float theta) {
|
void mp_matrix4_rotate(mp_matrix4 *dst, mp_vector3 *v, float theta) {
|
mp_matrix3 m3a;
|
mp_matrix3 m3a;
|
mp_matrix3_init(&m3a);
|
mp_matrix3_init(&m3a);
|
mp_matrix3_rotate0(&m3a, v, theta);
|
mp_matrix3_rotate0(&m3a, v, theta);
|
mp_matrix4_init(dst);
|
mp_matrix4_init(dst);
|
dst->a[0] = m3a.a[0];
|
dst->a[0] = m3a.a[0];
|
dst->a[1] = m3a.a[1];
|
dst->a[1] = m3a.a[1];
|
dst->a[2] = m3a.a[2];
|
dst->a[2] = m3a.a[2];
|
dst->a[4] = m3a.a[3];
|
dst->a[4] = m3a.a[3];
|
dst->a[5] = m3a.a[4];
|
dst->a[5] = m3a.a[4];
|
dst->a[6] = m3a.a[5];
|
dst->a[6] = m3a.a[5];
|
dst->a[8] = m3a.a[6];
|
dst->a[8] = m3a.a[6];
|
dst->a[9] = m3a.a[7];
|
dst->a[9] = m3a.a[7];
|
dst->a[10] = m3a.a[8];
|
dst->a[10] = m3a.a[8];
|
}
|
}
|
|
|
void mp_matrix4_get_elements(mp_matrix4 *dst, unsigned int *b) {
|
void mp_matrix4_get_elements(mp_matrix4 *dst, unsigned int *b) {
|
int i;
|
int i;
|
for (i = 0; i <16; i++) {
|
for (i = 0; i <16; i++) {
|
u_fui.f= dst->a[i];
|
u_fui.f= dst->a[i];
|
b[i] = u_fui.ui;
|
b[i] = u_fui.ui;
|
}
|
}
|
}
|
}
|
|
|
|
|