1 | #include "../Alloc/alloc.h"
|
---|
2 | #include "../../include/include.h"
|
---|
3 | #include "../Exceptions/exceptions.h"
|
---|
4 | #include "./isnan.h"
|
---|
5 | #include <math.h>
|
---|
6 |
|
---|
7 | void 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 | }
|
---|