diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_matrix.c')
| -rw-r--r-- | source/blender/blenlib/intern/math_matrix.c | 1093 |
1 files changed, 549 insertions, 544 deletions
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c index fd49012491e..e61a8ef041a 100644 --- a/source/blender/blenlib/intern/math_matrix.c +++ b/source/blender/blenlib/intern/math_matrix.c @@ -35,80 +35,80 @@ void zero_m3(float m[3][3]) { - memset(m, 0, 3*3*sizeof(float)); + memset(m, 0, 3 * 3 * sizeof(float)); } void zero_m4(float m[4][4]) { - memset(m, 0, 4*4*sizeof(float)); + memset(m, 0, 4 * 4 * sizeof(float)); } void unit_m3(float m[][3]) { - m[0][0]= m[1][1]= m[2][2]= 1.0; - m[0][1]= m[0][2]= 0.0; - m[1][0]= m[1][2]= 0.0; - m[2][0]= m[2][1]= 0.0; + m[0][0] = m[1][1] = m[2][2] = 1.0; + m[0][1] = m[0][2] = 0.0; + m[1][0] = m[1][2] = 0.0; + m[2][0] = m[2][1] = 0.0; } void unit_m4(float m[][4]) { - m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0; - m[0][1]= m[0][2]= m[0][3]= 0.0; - m[1][0]= m[1][2]= m[1][3]= 0.0; - m[2][0]= m[2][1]= m[2][3]= 0.0; - m[3][0]= m[3][1]= m[3][2]= 0.0; + m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0; + m[0][1] = m[0][2] = m[0][3] = 0.0; + m[1][0] = m[1][2] = m[1][3] = 0.0; + m[2][0] = m[2][1] = m[2][3] = 0.0; + m[3][0] = m[3][1] = m[3][2] = 0.0; } -void copy_m3_m3(float m1[][3], float m2[][3]) -{ +void copy_m3_m3(float m1[][3], float m2[][3]) +{ /* destination comes first: */ - memcpy(&m1[0], &m2[0], 9*sizeof(float)); + memcpy(&m1[0], &m2[0], 9 * sizeof(float)); } -void copy_m4_m4(float m1[][4], float m2[][4]) +void copy_m4_m4(float m1[][4], float m2[][4]) { - memcpy(m1, m2, 4*4*sizeof(float)); + memcpy(m1, m2, 4 * 4 * sizeof(float)); } void copy_m3_m4(float m1[][3], float m2[][4]) { - m1[0][0]= m2[0][0]; - m1[0][1]= m2[0][1]; - m1[0][2]= m2[0][2]; + m1[0][0] = m2[0][0]; + m1[0][1] = m2[0][1]; + m1[0][2] = m2[0][2]; - m1[1][0]= m2[1][0]; - m1[1][1]= m2[1][1]; - m1[1][2]= m2[1][2]; + m1[1][0] = m2[1][0]; + m1[1][1] = m2[1][1]; + m1[1][2] = m2[1][2]; - m1[2][0]= m2[2][0]; - m1[2][1]= m2[2][1]; - m1[2][2]= m2[2][2]; + m1[2][0] = m2[2][0]; + m1[2][1] = m2[2][1]; + m1[2][2] = m2[2][2]; } -void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */ +void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */ { - m1[0][0]= m2[0][0]; - m1[0][1]= m2[0][1]; - m1[0][2]= m2[0][2]; + m1[0][0] = m2[0][0]; + m1[0][1] = m2[0][1]; + m1[0][2] = m2[0][2]; - m1[1][0]= m2[1][0]; - m1[1][1]= m2[1][1]; - m1[1][2]= m2[1][2]; + m1[1][0] = m2[1][0]; + m1[1][1] = m2[1][1]; + m1[1][2] = m2[1][2]; - m1[2][0]= m2[2][0]; - m1[2][1]= m2[2][1]; - m1[2][2]= m2[2][2]; + m1[2][0] = m2[2][0]; + m1[2][1] = m2[2][1]; + m1[2][2] = m2[2][2]; /* Reevan's Bugfix */ - m1[0][3]=0.0F; - m1[1][3]=0.0F; - m1[2][3]=0.0F; + m1[0][3] = 0.0F; + m1[1][3] = 0.0F; + m1[2][3] = 0.0F; - m1[3][0]=0.0F; - m1[3][1]=0.0F; - m1[3][2]=0.0F; - m1[3][3]=1.0F; + m1[3][0] = 0.0F; + m1[3][1] = 0.0F; + m1[3][2] = 0.0F; + m1[3][3] = 1.0F; } @@ -119,7 +119,7 @@ void swap_m3m3(float m1[][3], float m2[][3]) for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { - t = m1[i][j]; + t = m1[i][j]; m1[i][j] = m2[i][j]; m2[i][j] = t; } @@ -133,7 +133,7 @@ void swap_m4m4(float m1[][4], float m2[][4]) for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { - t = m1[i][j]; + t = m1[i][j]; m1[i][j] = m2[i][j]; m2[i][j] = t; } @@ -151,25 +151,25 @@ void mult_m4_m4m4(float m1[][4], float m3_[][4], float m2_[][4]) copy_m4_m4(m3, m3_); /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */ - m1[0][0] = m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0] + m2[0][3]*m3[3][0]; - m1[0][1] = m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1] + m2[0][3]*m3[3][1]; - m1[0][2] = m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2] + m2[0][3]*m3[3][2]; - m1[0][3] = m2[0][0]*m3[0][3] + m2[0][1]*m3[1][3] + m2[0][2]*m3[2][3] + m2[0][3]*m3[3][3]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0] + m2[0][3] * m3[3][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1] + m2[0][3] * m3[3][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2] + m2[0][3] * m3[3][2]; + m1[0][3] = m2[0][0] * m3[0][3] + m2[0][1] * m3[1][3] + m2[0][2] * m3[2][3] + m2[0][3] * m3[3][3]; - m1[1][0] = m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0] + m2[1][3]*m3[3][0]; - m1[1][1] = m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1] + m2[1][3]*m3[3][1]; - m1[1][2] = m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2] + m2[1][3]*m3[3][2]; - m1[1][3] = m2[1][0]*m3[0][3] + m2[1][1]*m3[1][3] + m2[1][2]*m3[2][3] + m2[1][3]*m3[3][3]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0] + m2[1][3] * m3[3][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1] + m2[1][3] * m3[3][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2] + m2[1][3] * m3[3][2]; + m1[1][3] = m2[1][0] * m3[0][3] + m2[1][1] * m3[1][3] + m2[1][2] * m3[2][3] + m2[1][3] * m3[3][3]; - m1[2][0] = m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0] + m2[2][3]*m3[3][0]; - m1[2][1] = m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1] + m2[2][3]*m3[3][1]; - m1[2][2] = m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2] + m2[2][3]*m3[3][2]; - m1[2][3] = m2[2][0]*m3[0][3] + m2[2][1]*m3[1][3] + m2[2][2]*m3[2][3] + m2[2][3]*m3[3][3]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0] + m2[2][3] * m3[3][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1] + m2[2][3] * m3[3][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2] + m2[2][3] * m3[3][2]; + m1[2][3] = m2[2][0] * m3[0][3] + m2[2][1] * m3[1][3] + m2[2][2] * m3[2][3] + m2[2][3] * m3[3][3]; - m1[3][0] = m2[3][0]*m3[0][0] + m2[3][1]*m3[1][0] + m2[3][2]*m3[2][0] + m2[3][3]*m3[3][0]; - m1[3][1] = m2[3][0]*m3[0][1] + m2[3][1]*m3[1][1] + m2[3][2]*m3[2][1] + m2[3][3]*m3[3][1]; - m1[3][2] = m2[3][0]*m3[0][2] + m2[3][1]*m3[1][2] + m2[3][2]*m3[2][2] + m2[3][3]*m3[3][2]; - m1[3][3] = m2[3][0]*m3[0][3] + m2[3][1]*m3[1][3] + m2[3][2]*m3[2][3] + m2[3][3]*m3[3][3]; + m1[3][0] = m2[3][0] * m3[0][0] + m2[3][1] * m3[1][0] + m2[3][2] * m3[2][0] + m2[3][3] * m3[3][0]; + m1[3][1] = m2[3][0] * m3[0][1] + m2[3][1] * m3[1][1] + m2[3][2] * m3[2][1] + m2[3][3] * m3[3][1]; + m1[3][2] = m2[3][0] * m3[0][2] + m2[3][1] * m3[1][2] + m2[3][2] * m3[2][2] + m2[3][3] * m3[3][2]; + m1[3][3] = m2[3][0] * m3[0][3] + m2[3][1] * m3[1][3] + m2[3][2] * m3[2][3] + m2[3][3] * m3[3][3]; } @@ -181,18 +181,18 @@ void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3]) copy_m3_m3(m2, m2_); copy_m3_m3(m3, m3_); - /* m1[i][j] = m2[i][k]*m3[k][j], args are flipped! */ - m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; - m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; - m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; + /* m1[i][j] = m2[i][k] * m3[k][j], args are flipped! */ + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; - m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; - m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; - m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; - m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3]) @@ -203,56 +203,56 @@ void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3]) copy_m3_m3(m2, m2_); copy_m4_m4(m3, m3_); - m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; - m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; - m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; - m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; - m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; - m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; - m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; - m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; - m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } -/* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/ +/* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */ void mult_m3_m3m4(float m1[][3], float m3[][4], float m2[][3]) { /* m1[i][j] = m2[i][k] * m3[k][j] */ - m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0]; - m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1]; - m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0]; - m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1]; - m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0]; - m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1]; - m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4]) { - m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; - m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; - m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; - m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; - m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; - m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; - m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; - m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; - m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2]; } void mul_serie_m3(float answ[][3], - float m1[][3], float m2[][3], float m3[][3], - float m4[][3], float m5[][3], float m6[][3], - float m7[][3], float m8[][3]) + float m1[][3], float m2[][3], float m3[][3], + float m4[][3], float m5[][3], float m6[][3], + float m7[][3], float m8[][3]) { float temp[3][3]; - - if (m1==NULL || m2==NULL) return; - + + if (m1 == NULL || m2 == NULL) return; + mul_m3_m3m3(answ, m2, m1); if (m3) { mul_m3_m3m3(temp, m3, answ); @@ -278,14 +278,14 @@ void mul_serie_m3(float answ[][3], } void mul_serie_m4(float answ[][4], float m1[][4], - float m2[][4], float m3[][4], float m4[][4], - float m5[][4], float m6[][4], float m7[][4], - float m8[][4]) + float m2[][4], float m3[][4], float m4[][4], + float m5[][4], float m6[][4], float m7[][4], + float m8[][4]) { float temp[4][4]; - - if (m1==NULL || m2==NULL) return; - + + if (m1 == NULL || m2 == NULL) return; + mult_m4_m4m4(answ, m1, m2); if (m3) { mult_m4_m4m4(temp, answ, m3); @@ -312,41 +312,41 @@ void mul_serie_m4(float answ[][4], float m1[][4], void mul_m4_v3(float mat[][4], float vec[3]) { - float x,y; + float x, y; - x=vec[0]; - y=vec[1]; - vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; - vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; - vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; + vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; + vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; } void mul_v3_m4v3(float in[3], float mat[][4], const float vec[3]) { - float x,y; + float x, y; - x=vec[0]; - y=vec[1]; - in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; - in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; - in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; + x = vec[0]; + y = vec[1]; + in[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; + in[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; + in[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; } /* same as mul_m4_v3() but doesnt apply translation component */ void mul_mat3_m4_v3(float mat[][4], float vec[3]) { - float x,y; + float x, y; - x= vec[0]; - y= vec[1]; - vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]; - vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]; - vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2]; + vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2]; + vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2]; } void mul_project_m4_v3(float mat[][4], float vec[3]) { - const float w= vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3]; + const float w = vec[0] * mat[0][3] + vec[1] * mat[1][3] + vec[2] * mat[2][3] + mat[3][3]; mul_m4_v3(mat, vec); vec[0] /= w; @@ -358,14 +358,14 @@ void mul_v4_m4v4(float r[4], float mat[4][4], float v[4]) { float x, y, z; - x= v[0]; - y= v[1]; - z= v[2]; + x = v[0]; + y = v[1]; + z = v[2]; - r[0]= x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*v[3]; - r[1]= x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*v[3]; - r[2]= x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*v[3]; - r[3]= x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*v[3]; + r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3]; + r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3]; + r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3]; + r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3]; } void mul_m4_v4(float mat[4][4], float r[4]) @@ -377,14 +377,14 @@ void mul_v4d_m4v4d(double r[4], float mat[4][4], double v[4]) { double x, y, z; - x= v[0]; - y= v[1]; - z= v[2]; + x = v[0]; + y = v[1]; + z = v[2]; - r[0]= x*(double)mat[0][0] + y*(double)mat[1][0] + z*(double)mat[2][0] + (double)mat[3][0]*v[3]; - r[1]= x*(double)mat[0][1] + y*(double)mat[1][1] + z*(double)mat[2][1] + (double)mat[3][1]*v[3]; - r[2]= x*(double)mat[0][2] + y*(double)mat[1][2] + z*(double)mat[2][2] + (double)mat[3][2]*v[3]; - r[3]= x*(double)mat[0][3] + y*(double)mat[1][3] + z*(double)mat[2][3] + (double)mat[3][3]*v[3]; + r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3]; + r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3]; + r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + (double)mat[3][2] * v[3]; + r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + (double)mat[3][3] * v[3]; } void mul_m4_v4d(float mat[4][4], double r[4]) @@ -394,9 +394,9 @@ void mul_m4_v4d(float mat[4][4], double r[4]) void mul_v3_m3v3(float r[3], float M[3][3], float a[3]) { - r[0]= M[0][0]*a[0] + M[1][0]*a[1] + M[2][0]*a[2]; - r[1]= M[0][1]*a[0] + M[1][1]*a[1] + M[2][1]*a[2]; - r[2]= M[0][2]*a[0] + M[1][2]*a[1] + M[2][2]*a[2]; + r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2]; + r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2]; + r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2]; } void mul_m3_v3(float M[3][3], float r[3]) @@ -409,21 +409,21 @@ void mul_m3_v3(float M[3][3], float r[3]) void mul_transposed_m3_v3(float mat[][3], float vec[3]) { - float x,y; + float x, y; - x=vec[0]; - y=vec[1]; - vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2]; - vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2]; - vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2]; + vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2]; + vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2]; } void mul_m3_fl(float m[3][3], float f) { int i, j; - for (i=0;i<3;i++) - for (j=0;j<3;j++) + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) m[i][j] *= f; } @@ -431,8 +431,8 @@ void mul_m4_fl(float m[4][4], float f) { int i, j; - for (i=0;i<4;i++) - for (j=0;j<4;j++) + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) m[i][j] *= f; } @@ -440,56 +440,56 @@ void mul_mat3_m4_fl(float m[4][4], float f) { int i, j; - for (i=0; i<3; i++) - for (j=0; j<3; j++) + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) m[i][j] *= f; } void mul_m3_v3_double(float mat[][3], double vec[3]) { - double x,y; + double x, y; - x=vec[0]; - y=vec[1]; - vec[0]= x*(double)mat[0][0] + y*(double)mat[1][0] + (double)mat[2][0]*vec[2]; - vec[1]= x*(double)mat[0][1] + y*(double)mat[1][1] + (double)mat[2][1]*vec[2]; - vec[2]= x*(double)mat[0][2] + y*(double)mat[1][2] + (double)mat[2][2]*vec[2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2]; + vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2]; + vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2]; } void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) { int i, j; - for (i=0;i<3;i++) - for (j=0;j<3;j++) - m1[i][j]= m2[i][j] + m3[i][j]; + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + m1[i][j] = m2[i][j] + m3[i][j]; } void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) { int i, j; - for (i=0;i<4;i++) - for (j=0;j<4;j++) - m1[i][j]= m2[i][j] + m3[i][j]; + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) + m1[i][j] = m2[i][j] + m3[i][j]; } void sub_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) { int i, j; - for (i=0;i<3;i++) - for (j=0;j<3;j++) - m1[i][j]= m2[i][j] - m3[i][j]; + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + m1[i][j] = m2[i][j] - m3[i][j]; } void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) { int i, j; - for (i=0;i<4;i++) - for (j=0;j<4;j++) - m1[i][j]= m2[i][j] - m3[i][j]; + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) + m1[i][j] = m2[i][j] - m3[i][j]; } int invert_m3(float m[3][3]) @@ -497,7 +497,7 @@ int invert_m3(float m[3][3]) float tmp[3][3]; int success; - success= invert_m3_m3(tmp, m); + success = invert_m3_m3(tmp, m); copy_m3_m3(m, tmp); return success; @@ -509,20 +509,20 @@ int invert_m3_m3(float m1[3][3], float m2[3][3]) int a, b, success; /* calc adjoint */ - adjoint_m3_m3(m1,m2); + adjoint_m3_m3(m1, m2); /* then determinant old matrix! */ - det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1]) - -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1]) - +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]); + det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) - + m2[1][0] * (m2[0][1] * m2[2][2] - m2[0][2] * m2[2][1]) + + m2[2][0] * (m2[0][1] * m2[1][2] - m2[0][2] * m2[1][1])); - success= (det != 0); + success = (det != 0); - if (det==0) det=1; - det= 1/det; - for (a=0;a<3;a++) { - for (b=0;b<3;b++) { - m1[a][b]*=det; + if (det == 0) det = 1; + det = 1 / det; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m1[a][b] *= det; } } @@ -534,17 +534,17 @@ int invert_m4(float m[4][4]) float tmp[4][4]; int success; - success= invert_m4_m4(tmp, m); + success = invert_m4_m4(tmp, m); copy_m4_m4(m, tmp); return success; } /* - * invertmat - - * computes the inverse of mat and puts it in inverse. Returns - * TRUE on success (i.e. can always find a pivot) and FALSE on failure. - * Uses Gaussian Elimination with partial (maximal column) pivoting. + * invertmat - + * computes the inverse of mat and puts it in inverse. Returns + * TRUE on success (i.e. can always find a pivot) and FALSE on failure. + * Uses Gaussian Elimination with partial (maximal column) pivoting. * * Mark Segal - 1992 */ @@ -558,15 +558,15 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4]) int maxj; /* Set inverse to identity */ - for (i=0; i<4; i++) - for (j=0; j<4; j++) + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) inverse[i][j] = 0; - for (i=0; i<4; i++) + for (i = 0; i < 4; i++) inverse[i][i] = 1; /* Copy original matrix so we don't mess it up */ for (i = 0; i < 4; i++) - for (j = 0; j <4; j++) + for (j = 0; j < 4; j++) tempmat[i][j] = mat[i][j]; for (i = 0; i < 4; i++) { @@ -591,15 +591,15 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4]) if (temp == 0) return 0; /* No non-zero pivot */ for (k = 0; k < 4; k++) { - tempmat[i][k] = (float)(tempmat[i][k]/temp); - inverse[i][k] = (float)(inverse[i][k]/temp); + tempmat[i][k] = (float)(tempmat[i][k] / temp); + inverse[i][k] = (float)(inverse[i][k] / temp); } for (j = 0; j < 4; j++) { if (j != i) { temp = tempmat[j][i]; for (k = 0; k < 4; k++) { - tempmat[j][k] -= (float)(tempmat[i][k]*temp); - inverse[j][k] -= (float)(inverse[i][k]*temp); + tempmat[j][k] -= (float)(tempmat[i][k] * temp); + inverse[j][k] -= (float)(inverse[i][k] * temp); } } } @@ -655,8 +655,7 @@ void orthogonalize_m3(float mat[][3], int axis) float size[3]; mat3_to_size(size, mat); normalize_v3(mat[axis]); - switch(axis) - { + switch (axis) { case 0: if (dot_v3v3(mat[0], mat[1]) < 1) { cross_v3_v3v3(mat[2], mat[0], mat[1]); @@ -671,9 +670,9 @@ void orthogonalize_m3(float mat[][3], int axis) else { float vec[3]; - vec[0]= mat[0][1]; - vec[1]= mat[0][2]; - vec[2]= mat[0][0]; + vec[0] = mat[0][1]; + vec[1] = mat[0][2]; + vec[2] = mat[0][0]; cross_v3_v3v3(mat[2], mat[0], vec); normalize_v3(mat[2]); @@ -693,9 +692,9 @@ void orthogonalize_m3(float mat[][3], int axis) else { float vec[3]; - vec[0]= mat[1][1]; - vec[1]= mat[1][2]; - vec[2]= mat[1][0]; + vec[0] = mat[1][1]; + vec[1] = mat[1][2]; + vec[2] = mat[1][0]; cross_v3_v3v3(mat[0], mat[1], vec); normalize_v3(mat[0]); @@ -715,9 +714,9 @@ void orthogonalize_m3(float mat[][3], int axis) else { float vec[3]; - vec[0]= mat[2][1]; - vec[1]= mat[2][2]; - vec[2]= mat[2][0]; + vec[0] = mat[2][1]; + vec[1] = mat[2][2]; + vec[2] = mat[2][0]; cross_v3_v3v3(mat[0], vec, mat[2]); normalize_v3(mat[0]); @@ -734,8 +733,7 @@ void orthogonalize_m4(float mat[][4], int axis) float size[3]; mat4_to_size(size, mat); normalize_v3(mat[axis]); - switch(axis) - { + switch (axis) { case 0: if (dot_v3v3(mat[0], mat[1]) < 1) { cross_v3_v3v3(mat[2], mat[0], mat[1]); @@ -750,9 +748,9 @@ void orthogonalize_m4(float mat[][4], int axis) else { float vec[3]; - vec[0]= mat[0][1]; - vec[1]= mat[0][2]; - vec[2]= mat[0][0]; + vec[0] = mat[0][1]; + vec[1] = mat[0][2]; + vec[2] = mat[0][0]; cross_v3_v3v3(mat[2], mat[0], vec); normalize_v3(mat[2]); @@ -773,9 +771,9 @@ void orthogonalize_m4(float mat[][4], int axis) else { float vec[3]; - vec[0]= mat[1][1]; - vec[1]= mat[1][2]; - vec[2]= mat[1][0]; + vec[0] = mat[1][1]; + vec[1] = mat[1][2]; + vec[2] = mat[1][0]; cross_v3_v3v3(mat[0], mat[1], vec); normalize_v3(mat[0]); @@ -795,9 +793,9 @@ void orthogonalize_m4(float mat[][4], int axis) else { float vec[3]; - vec[0]= mat[2][1]; - vec[1]= mat[2][2]; - vec[2]= mat[2][0]; + vec[0] = mat[2][1]; + vec[1] = mat[2][2]; + vec[2] = mat[2][0]; cross_v3_v3v3(mat[0], vec, mat[2]); normalize_v3(mat[0]); @@ -844,121 +842,120 @@ int is_orthogonal_m4(float m[][4]) } void normalize_m3(float mat[][3]) -{ +{ normalize_v3(mat[0]); normalize_v3(mat[1]); normalize_v3(mat[2]); } void normalize_m3_m3(float rmat[][3], float mat[][3]) -{ +{ normalize_v3_v3(rmat[0], mat[0]); normalize_v3_v3(rmat[1], mat[1]); normalize_v3_v3(rmat[2], mat[2]); } - void normalize_m4(float mat[][4]) { float len; - - len= normalize_v3(mat[0]); - if (len!=0.0f) mat[0][3]/= len; - len= normalize_v3(mat[1]); - if (len!=0.0f) mat[1][3]/= len; - len= normalize_v3(mat[2]); - if (len!=0.0f) mat[2][3]/= len; + + len = normalize_v3(mat[0]); + if (len != 0.0f) mat[0][3] /= len; + len = normalize_v3(mat[1]); + if (len != 0.0f) mat[1][3] /= len; + len = normalize_v3(mat[2]); + if (len != 0.0f) mat[2][3] /= len; } void normalize_m4_m4(float rmat[][4], float mat[][4]) { float len; - - len= normalize_v3_v3(rmat[0], mat[0]); - if (len!=0.0f) rmat[0][3]= mat[0][3] / len; - len= normalize_v3_v3(rmat[1], mat[1]); - if (len!=0.0f) rmat[1][3]= mat[1][3] / len; - len= normalize_v3_v3(rmat[2], mat[2]); - if (len!=0.0f) rmat[2][3]= mat[2][3] / len; + + len = normalize_v3_v3(rmat[0], mat[0]); + if (len != 0.0f) rmat[0][3] = mat[0][3] / len; + len = normalize_v3_v3(rmat[1], mat[1]); + if (len != 0.0f) rmat[1][3] = mat[1][3] / len; + len = normalize_v3_v3(rmat[2], mat[2]); + if (len != 0.0f) rmat[2][3] = mat[2][3] / len; } void adjoint_m3_m3(float m1[][3], float m[][3]) { - m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1]; - m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1]; - m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1]; + m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1]; + m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1]; + m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1]; - m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0]; - m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0]; - m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0]; + m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0]; + m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0]; + m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0]; - m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0]; - m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0]; - m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0]; + m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0]; + m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0]; + m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0]; } -void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */ +void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */ { float a1, a2, a3, a4, b1, b2, b3, b4; float c1, c2, c3, c4, d1, d2, d3, d4; - a1= in[0][0]; - b1= in[0][1]; - c1= in[0][2]; - d1= in[0][3]; + a1 = in[0][0]; + b1 = in[0][1]; + c1 = in[0][2]; + d1 = in[0][3]; - a2= in[1][0]; - b2= in[1][1]; - c2= in[1][2]; - d2= in[1][3]; + a2 = in[1][0]; + b2 = in[1][1]; + c2 = in[1][2]; + d2 = in[1][3]; - a3= in[2][0]; - b3= in[2][1]; - c3= in[2][2]; - d3= in[2][3]; + a3 = in[2][0]; + b3 = in[2][1]; + c3 = in[2][2]; + d3 = in[2][3]; - a4= in[3][0]; - b4= in[3][1]; - c4= in[3][2]; - d4= in[3][3]; + a4 = in[3][0]; + b4 = in[3][1]; + c4 = in[3][2]; + d4 = in[3][3]; - out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4); - out[1][0] = - determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4); - out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4); - out[3][0] = - determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); + out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4); + out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4); + out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4); + out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); - out[0][1] = - determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4); - out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4); - out[2][1] = - determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4); - out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4); + out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4); + out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4); + out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4); + out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4); - out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4); - out[1][2] = - determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4); - out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4); - out[3][2] = - determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4); + out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4); + out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4); + out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4); + out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4); - out[0][3] = - determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3); - out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3); - out[2][3] = - determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3); - out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3); + out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3); + out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3); + out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3); + out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3); } -float determinant_m2(float a,float b,float c,float d) +float determinant_m2(float a, float b, float c, float d) { - return a*d - b*c; + return a * d - b * c; } float determinant_m3(float a1, float a2, float a3, - float b1, float b2, float b3, - float c1, float c2, float c3) + float b1, float b2, float b3, + float c1, float c2, float c3) { float ans; - ans = a1 * determinant_m2(b2, b3, c2, c3) - - b1 * determinant_m2(a2, a3, c2, c3) - + c1 * determinant_m2(a2, a3, b2, b3); + ans = (a1 * determinant_m2(b2, b3, c2, c3) - + b1 * determinant_m2(a2, a3, c2, c3) + + c1 * determinant_m2(a2, a3, b2, b3)); return ans; } @@ -966,32 +963,32 @@ float determinant_m3(float a1, float a2, float a3, float determinant_m4(float m[][4]) { float ans; - float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4; + float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; - a1= m[0][0]; - b1= m[0][1]; - c1= m[0][2]; - d1= m[0][3]; + a1 = m[0][0]; + b1 = m[0][1]; + c1 = m[0][2]; + d1 = m[0][3]; - a2= m[1][0]; - b2= m[1][1]; - c2= m[1][2]; - d2= m[1][3]; + a2 = m[1][0]; + b2 = m[1][1]; + c2 = m[1][2]; + d2 = m[1][3]; - a3= m[2][0]; - b3= m[2][1]; - c3= m[2][2]; - d3= m[2][3]; + a3 = m[2][0]; + b3 = m[2][1]; + c3 = m[2][2]; + d3 = m[2][3]; - a4= m[3][0]; - b4= m[3][1]; - c4= m[3][2]; - d4= m[3][3]; + a4 = m[3][0]; + b4 = m[3][1]; + c4 = m[3][2]; + d4 = m[3][3]; - ans = a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - - b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) - + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - - d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); + ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - + b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) + + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - + d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4)); return ans; } @@ -1000,38 +997,38 @@ float determinant_m4(float m[][4]) void size_to_mat3(float mat[][3], const float size[3]) { - mat[0][0]= size[0]; - mat[0][1]= 0.0f; - mat[0][2]= 0.0f; - mat[1][1]= size[1]; - mat[1][0]= 0.0f; - mat[1][2]= 0.0f; - mat[2][2]= size[2]; - mat[2][1]= 0.0f; - mat[2][0]= 0.0f; + mat[0][0] = size[0]; + mat[0][1] = 0.0f; + mat[0][2] = 0.0f; + mat[1][1] = size[1]; + mat[1][0] = 0.0f; + mat[1][2] = 0.0f; + mat[2][2] = size[2]; + mat[2][1] = 0.0f; + mat[2][0] = 0.0f; } void size_to_mat4(float mat[][4], const float size[3]) { float tmat[3][3]; - - size_to_mat3(tmat,size); + + size_to_mat3(tmat, size); unit_m4(mat); copy_m4_m3(mat, tmat); } void mat3_to_size(float size[3], float mat[][3]) { - size[0]= len_v3(mat[0]); - size[1]= len_v3(mat[1]); - size[2]= len_v3(mat[2]); + size[0] = len_v3(mat[0]); + size[1] = len_v3(mat[1]); + size[2] = len_v3(mat[2]); } void mat4_to_size(float size[3], float mat[][4]) { - size[0]= len_v3(mat[0]); - size[1]= len_v3(mat[1]); - size[2]= len_v3(mat[2]); + size[0] = len_v3(mat[0]); + size[1] = len_v3(mat[1]); + size[2] = len_v3(mat[2]); } /* this gets the average scale of a matrix, only use when your scaling @@ -1052,10 +1049,9 @@ float mat4_to_scale(float mat[][4]) return mat3_to_scale(tmat); } - void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3]) { - float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */ + float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */ float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */ /* rotation & scale are linked, we need to create the mat's @@ -1079,14 +1075,14 @@ void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3]) invert_m3_m3(imat3_n, mat3_n); mul_m3_m3m3(mat3, imat3_n, mat3); - size[0]= mat3[0][0]; - size[1]= mat3[1][1]; - size[2]= mat3[2][2]; + size[0] = mat3[0][0]; + size[1] = mat3[1][1]; + size[2] = mat3[2][2]; } void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[][4]) { - float mat3[3][3]; /* wmat -> 3x3 */ + float mat3[3][3]; /* wmat -> 3x3 */ copy_m3_m4(mat3, wmat); mat3_to_rot_size(rot, size, mat3); @@ -1097,33 +1093,33 @@ void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wm void scale_m3_fl(float m[][3], float scale) { - m[0][0]= m[1][1]= m[2][2]= scale; - m[0][1]= m[0][2]= 0.0; - m[1][0]= m[1][2]= 0.0; - m[2][0]= m[2][1]= 0.0; + m[0][0] = m[1][1] = m[2][2] = scale; + m[0][1] = m[0][2] = 0.0; + m[1][0] = m[1][2] = 0.0; + m[2][0] = m[2][1] = 0.0; } void scale_m4_fl(float m[][4], float scale) { - m[0][0]= m[1][1]= m[2][2]= scale; - m[3][3]= 1.0; - m[0][1]= m[0][2]= m[0][3]= 0.0; - m[1][0]= m[1][2]= m[1][3]= 0.0; - m[2][0]= m[2][1]= m[2][3]= 0.0; - m[3][0]= m[3][1]= m[3][2]= 0.0; + m[0][0] = m[1][1] = m[2][2] = scale; + m[3][3] = 1.0; + m[0][1] = m[0][2] = m[0][3] = 0.0; + m[1][0] = m[1][2] = m[1][3] = 0.0; + m[2][0] = m[2][1] = m[2][3] = 0.0; + m[3][0] = m[3][1] = m[3][2] = 0.0; } -void translate_m4(float mat[][4],float Tx, float Ty, float Tz) +void translate_m4(float mat[][4], float Tx, float Ty, float Tz) { - mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]); - mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]); - mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]); + mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]); + mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]); + mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]); } void rotate_m4(float mat[][4], const char axis, const float angle) { int col; - float temp[4]= {0.0f, 0.0f, 0.0f, 0.0f}; + float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f}; float cosine, sine; assert(axis >= 'X' && axis <= 'Z'); @@ -1131,32 +1127,32 @@ void rotate_m4(float mat[][4], const char axis, const float angle) cosine = (float)cos(angle); sine = (float)sin(angle); switch (axis) { - case 'X': - for (col=0 ; col<4 ; col++) - temp[col] = cosine*mat[1][col] + sine*mat[2][col]; - for (col=0 ; col<4 ; col++) { - mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col]; - mat[1][col] = temp[col]; - } - break; - - case 'Y': - for (col=0 ; col<4 ; col++) - temp[col] = cosine*mat[0][col] - sine*mat[2][col]; - for (col=0 ; col<4 ; col++) { - mat[2][col] = sine*mat[0][col] + cosine*mat[2][col]; - mat[0][col] = temp[col]; - } - break; - - case 'Z': - for (col=0 ; col<4 ; col++) - temp[col] = cosine*mat[0][col] + sine*mat[1][col]; - for (col=0 ; col<4 ; col++) { - mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col]; - mat[0][col] = temp[col]; - } - break; + case 'X': + for (col = 0; col < 4; col++) + temp[col] = cosine * mat[1][col] + sine * mat[2][col]; + for (col = 0; col < 4; col++) { + mat[2][col] = -sine * mat[1][col] + cosine * mat[2][col]; + mat[1][col] = temp[col]; + } + break; + + case 'Y': + for (col = 0; col < 4; col++) + temp[col] = cosine * mat[0][col] - sine * mat[2][col]; + for (col = 0; col < 4; col++) { + mat[2][col] = sine * mat[0][col] + cosine * mat[2][col]; + mat[0][col] = temp[col]; + } + break; + + case 'Z': + for (col = 0; col < 4; col++) + temp[col] = cosine * mat[0][col] + sine * mat[1][col]; + for (col = 0; col < 4; col++) { + mat[1][col] = -sine * mat[0][col] + cosine * mat[1][col]; + mat[0][col] = temp[col]; + } + break; } } @@ -1166,7 +1162,7 @@ void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float s float squat[4], dquat[4], fquat[4]; float sscale[3], dscale[3], fsize[3]; float rmat[3][3], smat[3][3]; - + mat3_to_rot_size(drot, dscale, dst); mat3_to_rot_size(srot, sscale, src); @@ -1178,8 +1174,8 @@ void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float s interp_v3_v3v3(fsize, dscale, sscale, srcweight); /* compose new matrix */ - quat_to_mat3(rmat,fquat); - size_to_mat3(smat,fsize); + quat_to_mat3(rmat, fquat); + size_to_mat3(smat, fsize); mul_m3_m3m3(out, rmat, smat); } @@ -1205,7 +1201,6 @@ void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float s loc_quat_size_to_mat4(out, floc, fquat, fsize); } - int is_negative_m3(float mat[][3]) { float vec[3]; @@ -1223,21 +1218,22 @@ int is_negative_m4(float mat[][4]) /* make a 4x4 matrix out of 3 transform components */ /* matrices are made in the order: scale * rot * loc */ // TODO: need to have a version that allows for rotation order... + void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3]) { float rmat[3][3], smat[3][3], tmat[3][3]; - + /* initialize new matrix */ unit_m4(mat); - + /* make rotation + scaling part */ - eul_to_mat3(rmat,eul); - size_to_mat3(smat,size); + eul_to_mat3(rmat, eul); + size_to_mat3(smat, size); mul_m3_m3m3(tmat, rmat, smat); - + /* copy rot/scale part to output matrix*/ copy_m4_m3(mat, tmat); - + /* copy location to matrix */ mat[3][0] = loc[0]; mat[3][1] = loc[1]; @@ -1245,22 +1241,23 @@ void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3 } /* make a 4x4 matrix out of 3 transform components */ + /* matrices are made in the order: scale * rot * loc */ void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3], const short rotOrder) { float rmat[3][3], smat[3][3], tmat[3][3]; - + /* initialize new matrix */ unit_m4(mat); - + /* make rotation + scaling part */ - eulO_to_mat3(rmat,eul, rotOrder); - size_to_mat3(smat,size); + eulO_to_mat3(rmat, eul, rotOrder); + size_to_mat3(smat, size); mul_m3_m3m3(tmat, rmat, smat); - + /* copy rot/scale part to output matrix*/ copy_m4_m3(mat, tmat); - + /* copy location to matrix */ mat[3][0] = loc[0]; mat[3][1] = loc[1]; @@ -1269,22 +1266,23 @@ void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[ /* make a 4x4 matrix out of 3 transform components */ + /* matrices are made in the order: scale * rot * loc */ void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3]) { float rmat[3][3], smat[3][3], tmat[3][3]; - + /* initialize new matrix */ unit_m4(mat); - + /* make rotation + scaling part */ - quat_to_mat3(rmat,quat); - size_to_mat3(smat,size); + quat_to_mat3(rmat, quat); + size_to_mat3(smat, size); mul_m3_m3m3(tmat, rmat, smat); - + /* copy rot/scale part to output matrix*/ copy_m4_m3(mat, tmat); - + /* copy location to matrix */ mat[3][0] = loc[0]; mat[3][1] = loc[1]; @@ -1303,19 +1301,19 @@ void loc_axisangle_size_to_mat4(float mat[4][4], const float loc[3], const float void print_m3(const char *str, float m[][3]) { printf("%s\n", str); - printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]); - printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]); - printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]); + printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]); + printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]); + printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]); printf("\n"); } void print_m4(const char *str, float m[][4]) { printf("%s\n", str); - printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]); - printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]); - printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]); - printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]); + printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]); + printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]); + printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]); + printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]); printf("\n"); } @@ -1323,9 +1321,9 @@ void print_m4(const char *str, float m[][4]) * from TNT matrix library * * Compute the Single Value Decomposition of an arbitrary matrix A - * That is compute the 3 matrices U,W,V with U column orthogonal (m,n) - * ,W a diagonal matrix and V an orthogonal square matrix s.t. - * A = U.W.Vt. From this decomposition it is trivial to compute the + * That is compute the 3 matrices U,W,V with U column orthogonal (m,n) + * ,W a diagonal matrix and V an orthogonal square matrix s.t. + * A = U.W.Vt. From this decomposition it is trivial to compute the * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U). */ @@ -1336,25 +1334,25 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) int m = 4; int n = 4; int maxiter = 200; - int nu = minf(m,n); + int nu = minf(m, n); float *work = work1; float *e = work2; float eps; - int i=0, j=0, k=0, p, pp, iter; + int i = 0, j = 0, k = 0, p, pp, iter; // Reduce A to bidiagonal form, storing the diagonal elements // in s and the super-diagonal elements in e. - int nct = minf(m-1,n); - int nrt = maxf(0,minf(n-2,m)); + int nct = minf(m - 1, n); + int nrt = maxf(0, minf(n - 2, m)); copy_m4_m4(A, A_); zero_m4(U); zero_v4(s); - for (k = 0; k < maxf(nct,nrt); k++) { + for (k = 0; k < maxf(nct, nrt); k++) { if (k < nct) { // Compute the transformation for the k-th column and @@ -1362,14 +1360,14 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Compute 2-norm of k-th column without under/overflow. s[k] = 0; for (i = k; i < m; i++) { - s[k] = hypotf(s[k],A[i][k]); + s[k] = hypotf(s[k], A[i][k]); } if (s[k] != 0.0f) { float invsk; if (A[k][k] < 0.0f) { s[k] = -s[k]; } - invsk = 1.0f/s[k]; + invsk = 1.0f / s[k]; for (i = k; i < m; i++) { A[i][k] *= invsk; } @@ -1377,18 +1375,18 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } s[k] = -s[k]; } - for (j = k+1; j < n; j++) { + for (j = k + 1; j < n; j++) { if ((k < nct) && (s[k] != 0.0f)) { - // Apply the transformation. + // Apply the transformation. float t = 0; for (i = k; i < m; i++) { - t += A[i][k]*A[i][j]; + t += A[i][k] * A[i][j]; } - t = -t/A[k][k]; + t = -t / A[k][k]; for (i = k; i < m; i++) { - A[i][j] += t*A[i][k]; + A[i][j] += t * A[i][k]; } } @@ -1411,39 +1409,39 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // k-th super-diagonal in e[k]. // Compute 2-norm without under/overflow. e[k] = 0; - for (i = k+1; i < n; i++) { - e[k] = hypotf(e[k],e[i]); + for (i = k + 1; i < n; i++) { + e[k] = hypotf(e[k], e[i]); } if (e[k] != 0.0f) { float invek; - if (e[k+1] < 0.0f) { + if (e[k + 1] < 0.0f) { e[k] = -e[k]; } - invek = 1.0f/e[k]; - for (i = k+1; i < n; i++) { + invek = 1.0f / e[k]; + for (i = k + 1; i < n; i++) { e[i] *= invek; } - e[k+1] += 1.0f; + e[k + 1] += 1.0f; } e[k] = -e[k]; - if ((k+1 < m) & (e[k] != 0.0f)) { + if ((k + 1 < m) & (e[k] != 0.0f)) { float invek1; - // Apply the transformation. + // Apply the transformation. - for (i = k+1; i < m; i++) { + for (i = k + 1; i < m; i++) { work[i] = 0.0f; } - for (j = k+1; j < n; j++) { - for (i = k+1; i < m; i++) { - work[i] += e[j]*A[i][j]; + for (j = k + 1; j < n; j++) { + for (i = k + 1; i < m; i++) { + work[i] += e[j] * A[i][j]; } } - invek1 = 1.0f/e[k+1]; - for (j = k+1; j < n; j++) { - float t = -e[j]*invek1; - for (i = k+1; i < m; i++) { - A[i][j] += t*work[i]; + invek1 = 1.0f / e[k + 1]; + for (j = k + 1; j < n; j++) { + float t = -e[j] * invek1; + for (i = k + 1; i < m; i++) { + A[i][j] += t * work[i]; } } } @@ -1451,24 +1449,24 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Place the transformation in V for subsequent // back multiplication. - for (i = k+1; i < n; i++) + for (i = k + 1; i < n; i++) V[i][k] = e[i]; } } // Set up the final bidiagonal matrix or order p. - p = minf(n,m+1); + p = minf(n, m + 1); if (nct < n) { s[nct] = A[nct][nct]; } if (m < p) { - s[p-1] = 0.0f; + s[p - 1] = 0.0f; } - if (nrt+1 < p) { - e[nrt] = A[nrt][p-1]; + if (nrt + 1 < p) { + e[nrt] = A[nrt][p - 1]; } - e[p-1] = 0.0f; + e[p - 1] = 0.0f; // If required, generate U. @@ -1478,23 +1476,23 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } U[j][j] = 1.0f; } - for (k = nct-1; k >= 0; k--) { + for (k = nct - 1; k >= 0; k--) { if (s[k] != 0.0f) { - for (j = k+1; j < nu; j++) { + for (j = k + 1; j < nu; j++) { float t = 0; for (i = k; i < m; i++) { - t += U[i][k]*U[i][j]; + t += U[i][k] * U[i][j]; } - t = -t/U[k][k]; + t = -t / U[k][k]; for (i = k; i < m; i++) { - U[i][j] += t*U[i][k]; + U[i][j] += t * U[i][k]; } } - for (i = k; i < m; i++ ) { + for (i = k; i < m; i++) { U[i][k] = -U[i][k]; } U[k][k] = 1.0f + U[k][k]; - for (i = 0; i < k-1; i++) { + for (i = 0; i < k - 1; i++) { U[i][k] = 0.0f; } } @@ -1508,16 +1506,16 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // If required, generate V. - for (k = n-1; k >= 0; k--) { + for (k = n - 1; k >= 0; k--) { if ((k < nrt) & (e[k] != 0.0f)) { - for (j = k+1; j < nu; j++) { + for (j = k + 1; j < nu; j++) { float t = 0; - for (i = k+1; i < n; i++) { - t += V[i][k]*V[i][j]; + for (i = k + 1; i < n; i++) { + t += V[i][k] * V[i][j]; } - t = -t/V[k+1][k]; - for (i = k+1; i < n; i++) { - V[i][j] += t*V[i][k]; + t = -t / V[k + 1][k]; + for (i = k + 1; i < n; i++) { + V[i][j] += t * V[i][k]; } } } @@ -1529,11 +1527,11 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Main iteration loop for the singular values. - pp = p-1; + pp = p - 1; iter = 0; - eps = powf(2.0f,-52.0f); + eps = powf(2.0f, -52.0f); while (p > 0) { - int kase=0; + int kase = 0; // Test for maximum iterations to avoid infinite loop if (maxiter == 0) @@ -1544,34 +1542,34 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // negligible elements in the s and e arrays. On // completion the variables kase and k are set as follows. - // kase = 1 if s(p) and e[k-1] are negligible and k<p + // kase = 1 if s(p) and e[k - 1] are negligible and k<p // kase = 2 if s(k) is negligible and k<p - // kase = 3 if e[k-1] is negligible, k<p, and - // s(k), ..., s(p) are not negligible (qr step). - // kase = 4 if e(p-1) is negligible (convergence). + // kase = 3 if e[k - 1] is negligible, k<p, and + // s(k), ..., s(p) are not negligible (qr step). + // kase = 4 if e(p - 1) is negligible (convergence). - for (k = p-2; k >= -1; k--) { + for (k = p - 2; k >= -1; k--) { if (k == -1) { break; } - if (fabsf(e[k]) <= eps*(fabsf(s[k]) + fabsf(s[k+1]))) { + if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) { e[k] = 0.0f; break; } } - if (k == p-2) { + if (k == p - 2) { kase = 4; } else { int ks; - for (ks = p-1; ks >= k; ks--) { + for (ks = p - 1; ks >= k; ks--) { float t; if (ks == k) { break; } - t = (ks != p ? fabsf(e[ks]) : 0.f) + - (ks != k+1 ? fabsf(e[ks-1]) : 0.0f); - if (fabsf(s[ks]) <= eps*t) { + t = (ks != p ? fabsf(e[ks]) : 0.f) + + (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f); + if (fabsf(s[ks]) <= eps * t) { s[ks] = 0.0f; break; } @@ -1579,7 +1577,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) if (ks == k) { kase = 3; } - else if (ks == p-1) { + else if (ks == p - 1) { kase = 1; } else { @@ -1595,127 +1593,130 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Deflate negligible s(p). - case 1: { - float f = e[p-2]; - e[p-2] = 0.0f; - for (j = p-2; j >= k; j--) { - float t = hypotf(s[j],f); - float invt = 1.0f/t; - float cs = s[j]*invt; - float sn = f*invt; + case 1: + { + float f = e[p - 2]; + e[p - 2] = 0.0f; + for (j = p - 2; j >= k; j--) { + float t = hypotf(s[j], f); + float invt = 1.0f / t; + float cs = s[j] * invt; + float sn = f * invt; s[j] = t; if (j != k) { - f = -sn*e[j-1]; - e[j-1] = cs*e[j-1]; + f = -sn * e[j - 1]; + e[j - 1] = cs * e[j - 1]; } for (i = 0; i < n; i++) { - t = cs*V[i][j] + sn*V[i][p-1]; - V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1]; + t = cs * V[i][j] + sn * V[i][p - 1]; + V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1]; V[i][j] = t; } } + break; } - break; // Split at negligible s(k). - case 2: { - float f = e[k-1]; - e[k-1] = 0.0f; + case 2: + { + float f = e[k - 1]; + e[k - 1] = 0.0f; for (j = k; j < p; j++) { - float t = hypotf(s[j],f); - float invt = 1.0f/t; - float cs = s[j]*invt; - float sn = f*invt; + float t = hypotf(s[j], f); + float invt = 1.0f / t; + float cs = s[j] * invt; + float sn = f * invt; s[j] = t; - f = -sn*e[j]; - e[j] = cs*e[j]; + f = -sn * e[j]; + e[j] = cs * e[j]; for (i = 0; i < m; i++) { - t = cs*U[i][j] + sn*U[i][k-1]; - U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1]; + t = cs * U[i][j] + sn * U[i][k - 1]; + U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1]; U[i][j] = t; } } + break; } - break; // Perform one qr step. - case 3: { + case 3: + { // Calculate the shift. float scale = maxf(maxf(maxf(maxf( - fabsf(s[p-1]),fabsf(s[p-2])),fabsf(e[p-2])), - fabsf(s[k])),fabsf(e[k])); - float invscale = 1.0f/scale; - float sp = s[p-1]*invscale; - float spm1 = s[p-2]*invscale; - float epm1 = e[p-2]*invscale; - float sk = s[k]*invscale; - float ek = e[k]*invscale; - float b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)*0.5f; - float c = (sp*epm1)*(sp*epm1); + fabsf(s[p - 1]),fabsf(s[p - 2])),fabsf(e[p - 2])), + fabsf(s[k])),fabsf(e[k])); + float invscale = 1.0f / scale; + float sp = s[p - 1] * invscale; + float spm1 = s[p - 2] * invscale; + float epm1 = e[p - 2] * invscale; + float sk = s[k] * invscale; + float ek = e[k] * invscale; + float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f; + float c = (sp * epm1) * (sp * epm1); float shift = 0.0f; float f, g; if ((b != 0.0f) || (c != 0.0f)) { - shift = sqrtf(b*b + c); + shift = sqrtf(b * b + c); if (b < 0.0f) { shift = -shift; } - shift = c/(b + shift); + shift = c / (b + shift); } - f = (sk + sp)*(sk - sp) + shift; - g = sk*ek; + f = (sk + sp) * (sk - sp) + shift; + g = sk * ek; // Chase zeros. - for (j = k; j < p-1; j++) { - float t = hypotf(f,g); + for (j = k; j < p - 1; j++) { + float t = hypotf(f, g); /* division by zero checks added to avoid NaN (brecht) */ - float cs = (t == 0.0f)? 0.0f: f/t; - float sn = (t == 0.0f)? 0.0f: g/t; + float cs = (t == 0.0f) ? 0.0f : f / t; + float sn = (t == 0.0f) ? 0.0f : g / t; if (j != k) { - e[j-1] = t; + e[j - 1] = t; } - f = cs*s[j] + sn*e[j]; - e[j] = cs*e[j] - sn*s[j]; - g = sn*s[j+1]; - s[j+1] = cs*s[j+1]; + f = cs * s[j] + sn * e[j]; + e[j] = cs * e[j] - sn * s[j]; + g = sn * s[j + 1]; + s[j + 1] = cs * s[j + 1]; for (i = 0; i < n; i++) { - t = cs*V[i][j] + sn*V[i][j+1]; - V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1]; + t = cs * V[i][j] + sn * V[i][j + 1]; + V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1]; V[i][j] = t; } - t = hypotf(f,g); + t = hypotf(f, g); /* division by zero checks added to avoid NaN (brecht) */ - cs = (t == 0.0f)? 0.0f: f/t; - sn = (t == 0.0f)? 0.0f: g/t; + cs = (t == 0.0f) ? 0.0f : f / t; + sn = (t == 0.0f) ? 0.0f : g / t; s[j] = t; - f = cs*e[j] + sn*s[j+1]; - s[j+1] = -sn*e[j] + cs*s[j+1]; - g = sn*e[j+1]; - e[j+1] = cs*e[j+1]; - if (j < m-1) { + f = cs * e[j] + sn * s[j + 1]; + s[j + 1] = -sn * e[j] + cs * s[j + 1]; + g = sn * e[j + 1]; + e[j + 1] = cs * e[j + 1]; + if (j < m - 1) { for (i = 0; i < m; i++) { - t = cs*U[i][j] + sn*U[i][j+1]; - U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1]; + t = cs * U[i][j] + sn * U[i][j + 1]; + U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1]; U[i][j] = t; } } } - e[p-2] = f; + e[p - 2] = f; iter = iter + 1; + break; } - break; - // Convergence. - case 4: { + case 4: + { // Make the singular values positive. @@ -1730,28 +1731,32 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) while (k < pp) { float t; - if (s[k] >= s[k+1]) { + if (s[k] >= s[k + 1]) { break; } t = s[k]; - s[k] = s[k+1]; - s[k+1] = t; - if (k < n-1) { + s[k] = s[k + 1]; + s[k + 1] = t; + if (k < n - 1) { for (i = 0; i < n; i++) { - t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t; + t = V[i][k + 1]; + V[i][k + 1] = V[i][k]; + V[i][k] = t; } } - if (k < m-1) { + if (k < m - 1) { for (i = 0; i < m; i++) { - t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t; + t = U[i][k + 1]; + U[i][k + 1] = U[i][k]; + U[i][k] = t; } } k++; } iter = 0; p--; + break; } - break; } } } @@ -1769,8 +1774,8 @@ void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon) transpose_m4(V); zero_m4(Wm); - for (i=0; i<4; i++) - Wm[i][i]= (W[i] < epsilon)? 0.0f: 1.0f/W[i]; + for (i = 0; i < 4; i++) + Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i]; transpose_m4(V); |