diff options
Diffstat (limited to 'src/poser_daveortho.c')
| -rw-r--r-- | src/poser_daveortho.c | 40 |
1 files changed, 21 insertions, 19 deletions
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 4cf6dd8..2bf57c6 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -193,17 +193,19 @@ void OrthoSolve( FLT invXXt[3][3]; FLT SXt[2][3]; FLT M[2][3]; // Morph matrix! (2 by 3) - TRANSP(Xt,X,3,nPoints); - MUL(XXt,X,Xt,3,nPoints,3); - MUL(SXt,S,Xt,2,nPoints,3); - INV(invXXt,XXt,3,3); - MUL(M,SXt,invXXt,2,3,3); -//PRINT(M,2,3); - -// Double checking work -FLT S_morph[2][SENSORS_PER_OBJECT]; -MUL(S_morph,M,X,2,3,nPoints); -for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; } + TRANSP(Xt, X, 3, nPoints); + MUL(XXt, X, Xt, 3, nPoints, 3); + MUL(SXt, S, Xt, 2, nPoints, 3); + INV(invXXt, XXt, 3, 3); + MUL(M, SXt, invXXt, 2, 3, 3); + // PRINT(M,2,3); + + // Double checking work + FLT S_morph[2][SENSORS_PER_OBJECT]; + MUL(S_morph, M, X, 2, 3, nPoints); + for (i = 0; i < nPoints; i++) { + S_morph[0][i] += sbar[0]; + S_morph[1][i] += sbar[1]; } //-------------------- // Solve for the non-trivial vector @@ -218,10 +220,10 @@ for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; } FLT B[3][1] = { {0.0}, {0.0}, {1.0} }; FLT inv_uM[3][3]; FLT uf[3][1]; - INV(inv_uM,uM,3,3); - MUL(uf,inv_uM,B,3,3,1); - - //-------------------- + INV(inv_uM, uM, 3, 3); + MUL(uf, inv_uM, B, 3, 3, 1); + + //-------------------- // Solve for unit length vector // f that goes into the camera //-------------------- @@ -250,10 +252,10 @@ for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; } // uhat,rhat //-------------------- FLT uhat[2][1], rhat[2][1], fhat[2][1]; - MUL(fhat,M,f,2,3,1); - MUL(uhat,M,u,2,3,1); - MUL(rhat,M,r,2,3,1); - FLT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] ); + MUL(fhat, M, f, 2, 3, 1); + MUL(uhat, M, u, 2, 3, 1); + MUL(rhat, M, r, 2, 3, 1); + FLT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] ); FLT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] ); FLT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] ); FLT urhat_len = 0.5 * (uhat_len + rhat_len); |