12 static inline float clamp(float val, float min, float max) {
13 if(val < min) return min;
14 if(val > max) return max;
18 // Simple wrappers around library routines
19 static inline float abs(float f) { return (float)::fabs(f); }
20 static inline float sqrt(float f) { return (float)::sqrt(f); }
21 static inline float ceil(float f) { return (float)::ceil(f); }
22 static inline float sin(float f) { return (float)::sin(f); }
23 static inline float cos(float f) { return (float)::cos(f); }
24 static inline float tan(float f) { return (float)::tan(f); }
25 static inline float atan(float f) { return (float)::atan(f); }
26 static inline float atan2(float y, float x) { return (float)::atan2(y,x); }
27 static inline float asin(float f) { return (float)::asin(f); }
28 static inline float acos(float f) { return (float)::acos(f); }
29 static inline float exp(float f) { return (float)::exp(f); }
30 static inline float sqr(float f) { return f*f; }
32 // Takes two args and runs afoul of the Koenig rules.
33 static inline float pow(double base, double exp) { return (float)::pow(base, exp); }
35 // double variants of the above
36 static inline double abs(double f) { return ::fabs(f); }
37 static inline double sqrt(double f) { return ::sqrt(f); }
38 static inline double ceil(double f) { return ::ceil(f); }
39 static inline double sin(double f) { return ::sin(f); }
40 static inline double cos(double f) { return ::cos(f); }
41 static inline double tan(double f) { return ::tan(f); }
42 static inline double atan2(double y, double x) { return ::atan2(y,x); }
43 static inline double floor(double x) { return ::floor(x); }
45 // Some 3D vector stuff. In all cases, it is permissible for the
46 // "out" vector to be the same as one of the inputs.
47 static inline void set3(float* v, float* out) {
53 static inline float dot3(float* a, float* b) {
54 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
57 static inline void cross3(float* a, float* b, float* out) {
58 float ax=a[0], ay=a[1], az=a[2];
59 float bx=b[0], by=b[1], bz=b[2];
60 out[0] = ay*bz - by*az;
61 out[1] = az*bx - bz*ax;
62 out[2] = ax*by - bx*ay;
65 static inline void mul3(float scalar, float* v, float* out)
67 out[0] = scalar * v[0];
68 out[1] = scalar * v[1];
69 out[2] = scalar * v[2];
72 static inline void add3(float* a, float* b, float* out){
78 static inline void sub3(float* a, float* b, float* out) {
84 static inline float mag3(float* v) {
85 return sqrt(dot3(v, v));
88 static inline void unit3(float* v, float* out) {
89 float imag = 1/mag3(v);
93 // Matrix array convention: 0 1 2
97 // Multiply two matrices
98 static void mmul33(float* a, float* b, float* out) {
100 tmp[0] = a[0]*b[0] + a[1]*b[3] + a[2]*b[6];
101 tmp[3] = a[3]*b[0] + a[4]*b[3] + a[5]*b[6];
102 tmp[6] = a[6]*b[0] + a[7]*b[3] + a[8]*b[6];
104 tmp[1] = a[0]*b[1] + a[1]*b[4] + a[2]*b[7];
105 tmp[4] = a[3]*b[1] + a[4]*b[4] + a[5]*b[7];
106 tmp[7] = a[6]*b[1] + a[7]*b[4] + a[8]*b[7];
108 tmp[2] = a[0]*b[2] + a[1]*b[5] + a[2]*b[8];
109 tmp[5] = a[3]*b[2] + a[4]*b[5] + a[5]*b[8];
110 tmp[8] = a[6]*b[2] + a[7]*b[5] + a[8]*b[8];
112 for(int i=0; i<9; ++i)
116 // Multiply by vector
117 static inline void vmul33(float* m, float* v, float* out) {
118 float x = v[0], y = v[1], z = v[2];
119 out[0] = x*m[0] + y*m[1] + z*m[2];
120 out[1] = x*m[3] + y*m[4] + z*m[5];
121 out[2] = x*m[6] + y*m[7] + z*m[8];
124 // Multiply the vector by the matrix transpose. Or pre-multiply the
125 // matrix by v as a row vector. Same thing.
126 static inline void tmul33(float* m, float* v, float* out) {
127 float x = v[0], y = v[1], z = v[2];
128 out[0] = x*m[0] + y*m[3] + z*m[6];
129 out[1] = x*m[1] + y*m[4] + z*m[7];
130 out[2] = x*m[2] + y*m[5] + z*m[8];
134 static void invert33(float* m, float* out) {
135 // Compute the inverse as the adjoint matrix times 1/(det M).
136 // A, B ... I are the cofactors of a b c
139 float a=m[0], b=m[1], c=m[2];
140 float d=m[3], e=m[4], f=m[5];
141 float g=m[6], h=m[7], i=m[8];
143 float A = (e*i - h*f);
144 float B = -(d*i - g*f);
145 float C = (d*h - g*e);
146 float D = -(b*i - h*c);
147 float E = (a*i - g*c);
148 float F = -(a*h - g*b);
149 float G = (b*f - e*c);
150 float H = -(a*f - d*c);
151 float I = (a*e - d*b);
153 float id = 1/(a*A + b*B + c*C);
155 out[0] = id*A; out[1] = id*D; out[2] = id*G;
156 out[3] = id*B; out[4] = id*E; out[5] = id*H;
157 out[6] = id*C; out[7] = id*F; out[8] = id*I;
160 // Transpose matrix (for an orthonormal orientation matrix, this
161 // is the same as the inverse).
162 static inline void trans33(float* m, float* out) {
163 // 0 1 2 Elements 0, 4, and 8 are the same
164 // 3 4 5 Swap elements 1/3, 2/6, and 5/7
183 // Generates an orthonormal basis:
184 // xOut becomes the unit vector in the direction of x
185 // yOut is perpendicular to xOut in the x/y plane
186 // zOut becomes the unit vector: (xOut cross yOut)
187 static void ortho33(float* x, float* y,
188 float* xOut, float* yOut, float* zOut) {
194 cross3(xOut, y0, zOut);
196 cross3(zOut, xOut, yOut);
200 }; // namespace yasim