rsys

commit 300f505af95218f5144b876b654b8c2951aceb96
parent e0b1b8b3cfc62bedfa09e9143bf1292ad0f6bcf7
Author: vaplv <vaplv@free.fr>
Date:   Fri,  3 Apr 2015 11:18:42 +0200

Add the f33_basis function

Diffstat:
M
src/float33.h
 | 
20
++++++++++++++++++++

1 file changed, 20 insertions(+), 0 deletions(-)
diff --git a/src/float33.h b/src/float33.h
@@ -153,5 +153,25 @@ f33_rotation_roll(float dst[9], const float roll/* in radian */)
   return dst;
 }
 
+static INLINE float*
+f33_basis(float dst[9], const float normal[3])
+{
+  float a[3], b[3], x[3], y[3], len;
+  ASSERT(normal);
+  f3_cross(a, f3(a, 1.f, 0.f, 0.f), normal);
+  f3_cross(b, f3(b, 0.f, 1.f, 0.f), normal);
+  len = f3_normalize(x, f3_dot(a, a) > f3_dot(b, b) ? a : b);
+  if(len <= 0.f) {
+    ASSERT(0 && "Degenerated normal");
+    return f33_splat(dst, 0.f);
+  }
+  len = f3_normalize(y, f3_cross(y, normal, x));
+  ASSERT(len > 0);
+  f3_set(dst + 0, x);
+  f3_set(dst + 3, y);
+  f3_set(dst + 6, normal);
+  return dst;
+}
+
 #endif /* FLOAT33_H */