source: issm/trunk/src/c/shared/Numerics/XZvectorsToCoordinateSystem.cpp@ 13975

Last change on this file since 13975 was 13975, checked in by Mathieu Morlighem, 12 years ago

merged trunk-jpl and trunk for revision 13974

File size: 1.6 KB
Line 
1#include "../Alloc/alloc.h"
2#include "../../include/include.h"
3#include "../Exceptions/exceptions.h"
4#include "./isnan.h"
5#include <math.h>
6
7void XZvectorsToCoordinateSystem(IssmDouble* T,IssmDouble* xzvectors){
8
9 IssmDouble x[3],y[3],z[3];
10 IssmDouble x_norm, y_norm, z_norm;
11
12 for(int i=0;i<6;i++){
13 if(xIsNan<IssmDouble>(xzvectors[i])){
14 /*At least one NaN found: default to Id*/
15 T[0*3+0] = 1.0; T[0*3+1] = 0.0; T[0*3+2] = 0.0;
16 T[1*3+0] = 0.0; T[1*3+1] = 1.0; T[1*3+2] = 0.0;
17 T[2*3+0] = 0.0; T[2*3+1] = 0.0; T[2*3+2] = 1.0;
18
19 return;
20 }
21 }
22
23 /* get input {x} (vector in local x-z plane): */
24 x[0] = xzvectors[0];
25 x[1] = xzvectors[1];
26 x[2] = xzvectors[2];
27
28 /* get input {z} (local tangent plane normal vector): */
29 z[0] = xzvectors[3];
30 z[1] = xzvectors[4];
31 z[2] = xzvectors[5];
32
33 /* compute {y} = {z} x {x}: */
34 y[0] = x[2]*z[1] - x[1]*z[2];
35 y[1] = -x[2]*z[0] + x[0]*z[2];
36 y[2] = x[1]*z[0] - x[0]*z[1];
37
38 /* normalise {x}, {y} and {z} to form unit vectors {i_hat}, {j_hat} and {k_hat};
39 store in {x}, {y}, and {z}: */
40 x_norm = sqrt( x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
41 y_norm = sqrt( y[0]*y[0] + y[1]*y[1] + y[2]*y[2]);
42 z_norm = sqrt( z[0]*z[0] + z[1]*z[1] + z[2]*z[2]);
43
44 x[0] = x[0]/x_norm; x[1] = x[1]/x_norm; x[2] = x[2]/x_norm;
45 y[0] = y[0]/y_norm; y[1] = y[1]/y_norm; y[2] = y[2]/y_norm;
46 z[0] = z[0]/z_norm; z[1] = z[1]/z_norm; z[2] = z[2]/z_norm;
47
48 /* Tlg columns are just {i_hat}, {j_hat} and {k_hat}, respectively: */
49 T[0*3+0] = x[0]; T[0*3+1] = y[0]; T[0*3+2] = z[0];
50 T[1*3+0] = x[1]; T[1*3+1] = y[1]; T[1*3+2] = z[1];
51 T[2*3+0] = x[2]; T[2*3+1] = y[2]; T[2*3+2] = z[2];
52}
Note: See TracBrowser for help on using the repository browser.