Math Lib Reorganization

* New header and source files.
* Still need a few tweaks before switching code to use them.

13 files changed, 6092 insertions, 0 deletions

diff --git a/source/blender/blenlib/BLI_math.h b/source/blender/blenlib/BLI_math.h
new file mode 100644
index 00000000000..b1917bc27b7
--- /dev/null
+++ b/source/blender/blenlib/BLI_math.h
@@ -0,0 +1,64 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#ifndef BLI_MATH
+#define BLI_MATH
+
+/* Abbreviations:
+ *
+ * fl = float
+ * db = double
+ * v2 = vec2 = vector 2
+ * v3 = vec3 = vector 3
+ * v4 = vec4 = vector 4
+ * qt = quat = quaternion
+ * dq = dquat = dual quaternion
+ * m2 = mat2 = matrix 2x2
+ * m3 = mat3 = matrix 3x3
+ * m4 = mat4 = matrix 4x4
+ * eul = euler rotation
+ * eulO = euler with order
+ *
+ * Variable Names:
+ *
+ * f = single value
+ * a, b, c = vectors
+ * r = result vector
+ * A, B, C = matrices
+ * R = result matrix
+ *
+ */
+
+#include "BLI_math_base.h"
+#include "BLI_math_color.h"
+#include "BLI_math_geom.h"
+#include "BLI_math_matrix.h"
+#include "BLI_math_rotation.h"
+#include "BLI_math_vector.h"
+
+#endif /* BLI_MATH */
+
diff --git a/source/blender/blenlib/BLI_math_base.h b/source/blender/blenlib/BLI_math_base.h
new file mode 100644
index 00000000000..4e845ae35d9
--- /dev/null
+++ b/source/blender/blenlib/BLI_math_base.h
@@ -0,0 +1,156 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#ifndef BLI_MATH_BASE
+#define BLI_MATH_BASE
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef WIN32
+#define _USE_MATH_DEFINES
+#endif
+
+#include <math.h>
+
+#ifndef M_PI
+#define M_PI        3.14159265358979323846
+#endif
+#ifndef M_PI_2
+#define M_PI_2      1.57079632679489661923
+#endif
+#ifndef M_SQRT2
+#define M_SQRT2     1.41421356237309504880
+#endif
+#ifndef M_SQRT1_2
+#define M_SQRT1_2   0.70710678118654752440
+#endif
+#ifndef M_1_PI
+#define M_1_PI      0.318309886183790671538
+#endif
+#ifndef M_E
+#define M_E             2.7182818284590452354
+#endif
+#ifndef M_LOG2E
+#define M_LOG2E         1.4426950408889634074
+#endif
+#ifndef M_LOG10E
+#define M_LOG10E        0.43429448190325182765
+#endif
+#ifndef M_LN2
+#define M_LN2           0.69314718055994530942
+#endif
+#ifndef M_LN10
+#define M_LN10          2.30258509299404568402
+#endif
+
+#ifndef sqrtf
+#define sqrtf(a) ((float)sqrt(a))
+#endif
+#ifndef powf
+#define powf(a, b) ((float)pow(a, b))
+#endif
+#ifndef cosf
+#define cosf(a) ((float)cos(a))
+#endif
+#ifndef sinf
+#define sinf(a) ((float)sin(a))
+#endif
+#ifndef acosf
+#define acosf(a) ((float)acos(a))
+#endif
+#ifndef asinf
+#define asinf(a) ((float)asin(a))
+#endif
+#ifndef atan2f
+#define atan2f(a, b) ((float)atan2(a, b))
+#endif
+#ifndef tanf
+#define tanf(a) ((float)tan(a))
+#endif
+#ifndef atanf
+#define atanf(a) ((float)atan(a))
+#endif
+#ifndef floorf
+#define floorf(a) ((float)floor(a))
+#endif
+#ifndef ceilf
+#define ceilf(a) ((float)ceil(a))
+#endif
+#ifndef fabsf
+#define fabsf(a) ((float)fabs(a))
+#endif
+#ifndef logf
+#define logf(a) ((float)log(a))
+#endif
+#ifndef expf
+#define expf(a) ((float)exp(a))
+#endif
+#ifndef fmodf
+#define fmodf(a, b) ((float)fmod(a, b))
+#endif
+
+#ifdef WIN32
+#ifndef FREE_WINDOWS
+#define isnan(n) _isnan(n)
+#define finite _finite
+#endif
+#endif
+
+#ifndef SWAP
+#define SWAP(type, a, b)	{ type sw_ap; sw_ap=(a); (a)=(b); (b)=sw_ap; }
+#endif
+
+#ifndef CLAMP
+#define CLAMP(a, b, c)		if((a)<(b)) (a)=(b); else if((a)>(c)) (a)=(c)
+#endif
+
+/******************************* Float ******************************/
+
+float sqrt3f(float f);
+double sqrt3d(double d);
+
+float saacosf(float f);
+float saasinf(float f);
+float sasqrtf(float f);
+float saacos(float fac);
+float saasin(float fac);
+float sasqrt(float fac);
+
+float interpf(float a, float b, float t);
+
+float power_of_2(float f);
+
+float shell_angle_to_dist(float angle);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLI_MATH_BASE */
+
diff --git a/source/blender/blenlib/BLI_math_color.h b/source/blender/blenlib/BLI_math_color.h
new file mode 100644
index 00000000000..b2d14f3ecbd
--- /dev/null
+++ b/source/blender/blenlib/BLI_math_color.h
@@ -0,0 +1,66 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#ifndef BLI_MATH_COLOR
+#define BLI_MATH_COLOR
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BLI_CS_SMPTE	0
+#define BLI_CS_REC709	1
+#define BLI_CS_CIE		2
+
+/******************* Conversion to RGB ********************/
+
+void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b);
+void hex_to_rgb(char *hexcol, float *r, float *g, float *b);
+void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb);
+void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb);
+void xyz_to_rgb(float x, float y, float z, float *r, float *g, float *b, int colorspace);
+void cpack_to_rgb(unsigned int col, float *r, float *g, float *b);
+
+/***************** Conversion from RGB ********************/
+
+void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv);
+void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr);
+void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv);
+unsigned int rgb_to_cpack(float r, float g, float b);
+unsigned int hsv_to_cpack(float h, float s, float v);
+
+/************************** Other *************************/
+
+int constrain_rgb(float *r, float *g, float *b);
+void minmax_rgb(short c[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLI_MATH_COLOR */
+
diff --git a/source/blender/blenlib/BLI_math_geom.h b/source/blender/blenlib/BLI_math_geom.h
new file mode 100644
index 00000000000..05530bce0e5
--- /dev/null
+++ b/source/blender/blenlib/BLI_math_geom.h
@@ -0,0 +1,157 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#ifndef BLI_MATH_GEOM
+#define BLI_MATH_GEOM
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/********************************** Polygons *********************************/
+
+void cent_tri_v3(float r[3], float a[3], float b[3], float c[3]);
+void cent_quad_v3(float r[3], float a[3], float b[3], float c[3], float d[3]);
+
+float normal_tri_v3(float r[3], float a[3], float b[3], float c[3]);
+float normal_quad_v3(float r[3], float a[3], float b[3], float c[3], float d[3]);
+
+float area_tri_v2(float a[2], float b[2], float c[2]);
+float area_tri_v3(float a[3], float b[3], float c[3]);
+float area_quad_v3(float a[3], float b[3], float c[3], float d[3]);
+float area_poly_v3(int nr, float *verts, float normal[3]); // TODO float verts[][3]
+
+/********************************* Distance **********************************/
+
+float dist_to_line_v2(float p[2], float l1[2], float l2[2]);
+float dist_to_line_segment_v2(float p[2], float l1[2], float l2[2]);
+
+float dist_to_line_segment_v3(float p[3], float l1[3], float l2[3]);
+float closest_to_line_v3(float r[3], float p[3], float l1[3], float l2[3]);
+void closest_to_line_segment_v3(float r[3], float p[3], float l1[3], float l2[3]);
+
+/******************************* Intersection ********************************/
+
+/* TODO return values are not always first yet */
+/* TODO int return value consistency */
+
+/* line-line */
+#define ISECT_LINE_LINE_COLINEAR	-1
+#define ISECT_LINE_LINE_NONE		 0
+#define ISECT_LINE_LINE_EXACT		 1
+#define ISECT_LINE_LINE_CROSS		 2
+
+short isect_line_line_v2(float a1[2], float a2[2], float b1[2], float b2[2]); // TODO return int
+short isect_line_line_v2_short(short a1[2], short a2[2], short b1[2], short b2[2]); // TODO return int
+
+/* Returns the number of point of interests
+ * 0 - lines are colinear
+ * 1 - lines are coplanar, i1 is set to intersection
+ * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively 
+ * */
+
+int isect_line_line_v3(float v1[3], float v2[3],
+	float v3[3], float v4[3], float i1[3], float i2[3]);
+int isect_line_line_strict_v3(float v1[3], float v2[3],
+	float v3[3], float v4[3], float vi[3], float *lambda);
+
+/* line/ray triangle */
+int isect_line_tri_v3(float p1[3], float p2[3],
+	float v0[3], float v1[3], float v2[3], float *lambda, float *uv);
+int isect_ray_tri_v3(float p1[3], float d[3],
+	float v0[3], float v1[3], float v2[3], float *lambda, float *uv);
+int isect_ray_tri_threshold_v3(float p1[3], float d[3],
+	float v0[3], float v1[3], float v2[3], float *lambda, float *uv, float threshold);
+
+/* point in polygon */
+int isect_point_tri_v2(float p[2], float a[2], float b[2], float c[2]);
+int isect_point_quad_v2(float p[2], float a[2], float b[2], float c[2], float d[2]);
+
+int isect_point_tri_v2(float v1[2], float v2[2], float v3[2], float pt[2]);
+int isect_point_tri_v2_int(int x1, int y1, int x2, int y2, int a, int b);
+int isect_point_tri_prism_v3(float p[3], float v1[3], float v2[3], float v3[3]);
+
+void isect_point_quad_uv_v2(float v0[2], float v1[2], float v2[2], float v3[2],
+	float pt[2], float *uv);
+void isect_point_face_uv_v2(int isquad, float v0[2], float v1[2], float v2[2],
+	float v3[2], float pt[2], float *uv);
+
+/* other */
+int isect_sweeping_sphere_tri_v3(float p1[3], float p2[3], float radius,
+	float v0[3], float v1[3], float v2[3], float *lambda, float *ipoint);
+
+int isect_axial_line_tri_v3(int axis, float co1[3], float co2[3],
+	float v0[3], float v1[3], float v2[3], float *lambda);
+
+int isect_aabb_aabb_v3(float min1[3], float max1[3], float min2[3], float max2[3]);
+
+/****************************** Interpolation ********************************/
+
+/* tri or quad, d can be NULL */
+void interp_weights_face_v3(float w[4],
+	float a[3], float b[3], float c[3], float d[3], float p[3]);
+void interp_weights_poly_v3(float w[], float v[][3], int n, float p[3]);
+
+void interp_cubic_v3(float x[3], float v[3],
+	float x1[3], float v1[3], float x2[3], float v2[3], float t);
+
+/***************************** View & Projection *****************************/
+
+void lookat_m4(float mat[4][4], float vx, float vy, 
+	float vz, float px, float py, float pz, float twist);
+void polarview_m4(float mat[4][4], float dist, float azimuth,
+	float incidence, float twist);
+
+void perspective_m4(float mat[4][4], float left, float right,
+	float bottom, float top, float nearClip, float farClip);
+void orthographic_m4(float mat[4][4], float left, float right,
+	float bottom, float top, float nearClip, float farClip);
+
+/********************************** Mapping **********************************/
+
+void map_to_tube(float *u, float *v, float x, float y, float z);
+void map_to_sphere(float *u, float *v, float x, float y, float z);
+
+/********************************* Tangents **********************************/
+
+typedef struct VertexTangent {
+	float tang[3], uv[2];
+	struct VertexTangent *next;
+} VertexTangent;
+
+float *find_vertex_tangent(VertexTangent *vtang, float *uv);
+void sum_or_add_vertex_tangent(void *arena, VertexTangent **vtang,
+	float *tang, float *uv);
+void tangent_from_uv(float *uv1, float *uv2, float *uv3,
+	float *co1, float *co2, float *co3, float *n, float *tang);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLI_MATH_GEOM */
+
diff --git a/source/blender/blenlib/BLI_math_matrix.h b/source/blender/blenlib/BLI_math_matrix.h
new file mode 100644
index 00000000000..9a9c1be6258
--- /dev/null
+++ b/source/blender/blenlib/BLI_math_matrix.h
@@ -0,0 +1,165 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ 
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#ifndef BLI_MATH_MATRIX
+#define BLI_MATH_MATRIX
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/********************************* Init **************************************/
+
+#define MAT4_UNITY {{ 1.0, 0.0, 0.0, 0.0},\
+					{ 0.0, 1.0, 0.0, 0.0},\
+					{ 0.0, 0.0, 1.0, 0.0},\
+					{ 0.0, 0.0, 0.0, 1.0}}
+
+#define MAT3_UNITY {{ 1.0, 0.0, 0.0},\
+					{ 0.0, 1.0, 0.0},\
+					{ 0.0, 0.0, 1.0}}
+
+void zero_m3(float *R); // TODO R[3][3]);
+void zero_m4(float *R); // TODO R[4][4]);
+
+void unit_m3(float R[3][3]);
+void unit_m4(float R[4][4]);
+
+void copy_m3_m3(float R[3][3], float A[3][3]);
+void copy_m4_m4(float R[4][4], float A[4][4]);
+void copy_m3_m4(float R[3][3], float A[4][4]);
+void copy_m4_m3(float R[4][4], float A[3][3]);
+
+void swap_m3m3(float A[3][3], float B[3][3]);
+void swap_m4m4(float A[4][4], float B[4][4]);
+
+/******************************** Arithmetic *********************************/
+
+void add_m3_m3m3(float R[3][3], float A[3][3], float B[3][3]);
+void add_m4_m4m4(float R[4][4], float A[4][4], float B[4][4]);
+
+// TODO review mul order
+
+void mul_m3_m3m3(float R[3][3], float A[3][3], float B[3][3]);
+void mul_m4_m4m4(float R[4][4], float A[4][4], float B[4][4]);
+void mul_m4_m3m4(float R[4][4], float A[3][3], float B[4][4]);
+void mul_m4_m4m3(float R[4][4], float A[4][4], float B[3][3]);
+void mul_m3_m3m4(float R[3][3], float A[3][3], float B[4][4]);
+
+void mul_serie_m3(float R[3][3],
+	float M1[3][3], float M2[3][3], float M3[3][3], float M4[3][3],
+	float M5[3][3], float M6[3][3], float M7[3][3], float M8[3][3]);
+void mul_serie_m4(float R[4][4],
+	float M1[4][4], float M2[4][4], float M3[4][4], float M4[4][4],
+	float M5[4][4], float M6[4][4], float M7[4][4], float M8[4][4]);
+
+void mul_m4_v3(float M[4][4], float r[3]); // TODO order
+void mul_v3_m4v3(float r[3], float M[4][4], float v[3]);
+void mul_no_transl_m4v3(float M[4][4], float r[3]);
+void mul_m4_v4(float M[4][4], float r[3]); // TODO order
+void mul_project_m4_v4(float M[4][4], float r[3]); // TODO order
+
+void mul_m3_v3(float M[3][3], float r[3]); // TODO order
+void mul_transposed_m3_v3(float M[3][3], float r[3]); // TODO order
+void mul_m3_v3_double(float M[3][3], double r[3]);
+
+void mul_m3_fl(float *R, float f); // TODO R[3][3], float f);
+void mul_m4_fl(float *R, float f); // TODO R[4][4], float f);
+void mul_no_transl_m4_fl(float *R, float f); // TODO R[4][4], float f);
+
+void invert_m3(float R[3][3]);
+void invert_m3_m3(float R[3][3], float A[3][3]);
+void invert_m4(float R[4][4]); // TODO does not exist
+int invert_m4_m4(float R[4][4], float A[4][4]); // TODO return int
+
+/****************************** Linear Algebra *******************************/
+
+void transpose_m3(float R[3][3]);
+void transpose_m4(float R[4][4]);
+
+void normalize_m3(float R[3][3]);
+void normalize_m4(float R[4][4]);
+
+void orthogonalize_m3(float R[3][3], int axis);
+void orthogonalize_m4(float R[4][4], int axis);
+
+int is_orthogonal_m3(float mat[3][3]);
+int is_orthogonal_m4(float mat[4][4]);
+
+void adjoint_m3_m3(float R[3][3], float A[3][3]);
+void adjoint_m4_m4(float R[4][4], float A[4][4]);
+
+//float determinant_m2(float A[2][2]); // TODO params
+//float determinant_m3(float A[3][3]); // TODO params
+
+float determinant_m2(
+	float a, float b,
+	float c, float d);
+float determinant_m3(
+	float a, float b, float c,
+	float d, float e, float f,
+	float g, float h, float i);
+float determinant_m4(float A[4][4]);
+
+/****************************** Transformations ******************************/
+
+void scale_m3_fl(float R[3][3], float scale);
+void scale_m4_fl(float R[4][4], float scale);
+
+float mat3_to_scale(float M[3][3]);
+float mat4_to_scale(float M[4][4]);
+
+void size_to_mat3(float R[3][3], float size[3]);
+void size_to_mat4(float R[4][4], float size[3]);
+
+void mat3_to_size(float r[3], float M[3][3]);
+void mat4_to_size(float r[3], float M[4][4]);
+
+void translate_m4(float mat[4][4], float tx, float ty, float tz);
+void rotate_m4(float mat[4][4], char axis, float angle);
+
+void loc_eul_size_to_mat4(float R[4][4],
+	float loc[3], float eul[3], float size[3]);
+void loc_eulO_size_to_mat4(float R[4][4],
+	float loc[3], float eul[3], float size[3], short order);
+void loc_quat_size_to_mat4(float R[4][4],
+	float loc[3], float quat[4], float size[3]);
+
+void blend_m3_m3m3(float R[3][3], float A[3][3], float B[3][3], float t);
+void blend_m4_m4m4(float R[4][4], float A[4][4], float B[4][4], float t);
+
+/*********************************** Other ***********************************/
+
+void print_m3(char *str, float M[3][3]);
+void print_m4(char *str, float M[3][4]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLI_MATH_MATRIX */
+
diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h
new file mode 100644
index 00000000000..b1546e3db6c
--- /dev/null
+++ b/source/blender/blenlib/BLI_math_rotation.h
@@ -0,0 +1,172 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#ifndef BLI_MATH_ROTATION
+#define BLI_MATH_ROTATION
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define RAD2DEG(_rad) ((_rad)*(180.0/M_PI))
+#define DEG2RAD(_deg) ((_deg)*(M_PI/180.0))
+
+/******************************** Quaternions ********************************/
+/* stored in (w, x, y, z) order                                              */
+
+/* init */
+void unit_qt(float q[4]);
+void copy_qt_qt(float q[4], float a[4]);
+
+/* arithmetic */
+void mul_qt_qtqt(float q[4], float a[4], float b[4]);
+void mul_qt_v3(float q[4], float r[3]); // TODO order
+void mul_qt_fl(float q[4], float f);
+void mul_fac_qt_fl(float q[4], float f);
+
+void sub_qt_qtqt(float q[4], float a[4], float b[4]);
+
+void invert_qt(float q[4]);
+void conjugate_qt(float q[4]);
+float dot_qtqt(float a[4], float b[4]);
+void normalize_qt(float q[4]);
+
+/* comparison */
+int is_zero_qt(float q[4]);
+
+/* interpolation */
+void interp_qt_qtqt(float q[4], float a[4], float b[4], float t);
+void add_qt_qtqt(float q[4], float a[4], float b[4], float t); // TODO name
+
+/* conversion */
+void quat_to_mat3(float mat[3][3], float q[4]);
+void quat_to_mat4(float mat[4][4], float q[4]);
+
+void mat3_to_quat(float q[4], float mat[3][3]);
+void mat4_to_quat(float q[4], float mat[4][4]);
+void tri_to_quat(float q[4], float a[3], float b[3], float c[3]);
+void vec_to_quat(float q[4], float vec[3], short axis, short upflag);
+void rotation_between_vecs_to_quat(float q[4], float v1[3], float v2[3]);
+
+void Mat3ToQuat_is_ok(float wmat[][3], float *q); // TODO what is this?
+
+/* other */
+void print_qt(char *str, float q[4]);
+
+/******************************** Axis Angle *********************************/
+
+/* conversion */
+void axis_angle_to_quat(float r[4], float axis[3], float angle);
+void axis_angle_to_mat3(float R[3][3], float axis[3], float angle);
+void axis_angle_to_mat4(float R[4][4], float axis[3], float angle);
+
+void quat_to_axis_angle(float axis[3], float *angle, float q[4]);
+void mat3_to_axis_angle(float axis[3], float *angle, float M[3][3]);
+void mat4_to_axis_angle(float axis[3], float *angle, float M[4][4]);
+
+/****************************** Vector/Rotation ******************************/
+/* old axis angle code                                                       */
+/* TODO: the following calls should probably be depreceated sometime         */
+
+/* conversion */
+void mat3_to_vec_rot(float vec[3], float *phi, float mat[3][3]);
+void mat4_to_vec_rot(float vec[3], float *phi, float mat[4][4]);
+
+void vec_rot_to_quat(float quat[4], float vec[3], float phi); // TODO
+void vec_rot_to_mat3(float mat[3][3], float vec[3], float phi);
+void vec_rot_to_mat4(float mat[4][4], float vec[3], float phi);
+
+/******************************** XYZ Eulers *********************************/
+
+void eul_to_quat(float quat[4], float eul[3]);
+void eul_to_mat3(float mat[3][3], float eul[3]);
+void eul_to_mat4(float mat[4][4], float eul[3]);
+
+void quat_to_eul(float eul[3], float quat[4]);
+void mat3_to_eul(float eul[3], float mat[3][3]);
+void mat4_to_eul(float eul[3], float mat[4][4]);
+
+void compatible_eul(float eul[3], float old[3]);
+void mat3_to_compatible_eul(float eul[3], float old[3], float mat[3][3]);
+
+void rotate_eul(float eul[3], char axis, float angle);
+
+/************************** Arbitrary Order Eulers ***************************/
+
+/* warning: must match the eRotationModes in DNA_action_types.h
+ * order matters - types are saved to file. */
+
+typedef enum eEulerRotationOrders {
+	EULER_ORDER_DEFAULT = 1, /* blender classic = XYZ */
+	EULER_ORDER_XYZ = 1,
+	EULER_ORDER_XZY,
+	EULER_ORDER_YXZ,
+	EULER_ORDER_YZX,
+	EULER_ORDER_ZXY,
+	EULER_ORDER_ZYX,
+	/* there are 6 more entries with dulpicate entries included */
+} eEulerRotationOrders;
+
+void eulO_to_quat(float quat[4], float eul[3], short order);
+void eulO_to_mat3(float mat[3][3], float eul[3], short order);
+void eulO_to_mat4(float mat[4][4], float eul[3], short order);
+void eulO_to_axis_angle(float axis[3], float *angle, float eul[3], short order);
+void eulO_to_gimbal_axis(float gmat[3][3], float eul[3], short order);
+ 
+void quat_to_eulO(float eul[3], short order, float quat[4]);
+void mat3_to_eulO(float eul[3], short order, float mat[3][3]);
+void mat4_to_eulO(float eul[3], short order, float mat[4][4]);
+void axis_angle_to_eulO(float eul[3], short order, float axis[3], float angle);
+
+void mat3_to_compatible_eulO(float eul[3], float old[3], short order, float mat[3][3]);
+
+void rotate_eulO(float eul[3], short order, char axis, float angle);
+
+/******************************* Dual Quaternions ****************************/
+
+typedef struct DualQuat {
+	float quat[4];
+	float trans[4];
+
+	float scale[4][4];
+	float scale_weight;
+} DualQuat;
+
+void copy_dq_dq(DualQuat *r, DualQuat *dq);
+void normalize_dq(DualQuat *dq, float totw);
+void add_weighted_dq_dq(DualQuat *r, DualQuat *dq, float weight);
+void mul_v3m3_dq(float r[3], float R[3][3], DualQuat *dq);
+
+void mat4_to_dquat(DualQuat *r, float base[4][4], float M[4][4]);
+void dquat_to_mat4(float R[4][4], DualQuat *dq);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLI_MATH_ROTATION */
+
diff --git a/source/blender/blenlib/BLI_math_vector.h b/source/blender/blenlib/BLI_math_vector.h
new file mode 100644
index 00000000000..1cb1912208e
--- /dev/null
+++ b/source/blender/blenlib/BLI_math_vector.h
@@ -0,0 +1,140 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#ifndef BLI_MATH_VECTOR
+#define BLI_MATH_VECTOR
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MINLINE
+
+//#define static inline
+//#include "intern/math_vector_inline.h"
+
+/************************************* Init ***********************************/
+
+void zero_v2(float r[2]);
+void zero_v3(float r[3]);
+
+void copy_v2_v2(float r[2], float a[2]);
+void copy_v3_v3(float r[3], float a[3]);
+
+/********************************* Arithmetic ********************************/
+
+void add_v2_v2(float r[2], float a[2]);
+void add_v2_v2v2(float r[2], float a[2], float b[2]);
+void add_v3_v3(float r[3], float a[3]);
+void add_v3_v3v3(float r[3], float a[3], float b[3]);
+
+void sub_v2_v2(float r[2], float a[2]);
+void sub_v2_v2v2(float r[2], float a[2], float b[2]);
+void sub_v3_v3(float r[3], float a[3]);
+void sub_v3_v3v3(float r[3], float a[3], float b[3]);
+
+void mul_v2_fl(float r[2], float f);
+void mul_v3_fl(float r[3], float f);
+void mul_v3_v3fl(float r[3], float a[3], float f);
+void mul_v3_v3(float r[3], float a[3]);
+void mul_v3_v3v3(float r[3], float a[3], float b[3]);
+
+void negate_v3(float r[3]);
+void negate_v3_v3(float r[3], float a[3]);
+
+float dot_v2v2(float a[2], float b[2]); 
+float dot_v3v3(float a[3], float b[3]);
+
+float cross_v2v2(float a[2], float b[2]);
+void cross_v3_v3v3(float r[3], float a[3], float b[3]);
+
+void star_m3_v3(float R[3][3],float a[3]);
+
+/*********************************** Length **********************************/
+
+float len_v2(float a[2]);
+float len_v2v2(float a[2], float b[2]);
+float len_v3(float a[3]);
+float len_v3v3(float a[3], float b[3]);
+
+float normalize_v2(float r[2]);
+float normalize_v3(float r[3]);
+
+/******************************* Interpolation *******************************/
+
+void interp_v2_v2v2(float r[2], const float a[2], const float b[2], const float t); // TODO const
+void interp_v2_v2v2v2(float r[2], const float a[2], const float b[2], const float c[3], const float t[3]); // TODO const
+void interp_v3_v3v3(float r[3], const float a[3], const float b[3], const float t); // TODO const
+void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3]); // TODO const
+
+void mid_v3_v3v3(float r[3], float a[3], float b[3]);
+
+/********************************* Comparison ********************************/
+
+int is_zero_v3(float a[3]);
+int equals_v3v3(float a[3], float b[3]);
+int compare_v3v3(float a[3], float b[3], float limit);
+int compare_len_v3v3(float a[3], float b[3], float limit);
+
+int compare_v4v4(float a[4], float b[4], float limit);
+
+/********************************** Angles ***********************************/
+/* - angle with 2 arguments is angle between vector                          */
+/* - angle with 3 arguments is angle between 3 points at the middle point    */
+/* - angle_normalized_* is faster equivalent if vectors are normalized       */
+
+float angle_v2v2(float a[2], float b[2]);
+float angle_v2v2v2(float a[2], float b[2], float c[2]);
+float angle_normalized_v2v2(float a[2], float b[2]);
+float angle_v3v3(float a[2], float b[2]);
+float angle_v3v3v3(float a[2], float b[2], float c[2]);
+float angle_normalized_v3v3(float a[3], float b[3]);
+
+/********************************* Geometry **********************************/
+
+void project_v3_v3v3(float r[3], float p[3], float n[3]);
+void reflect_v3_v3v3(float r[3], float v[3], float n[3]);
+void ortho_basis_v3v3_v3(float r1[3], float r2[3], float a[3]);
+void bisect_v3_v3v3v3(float r[3], float a[3], float b[3], float c[3]);
+
+/*********************************** Other ***********************************/
+
+void print_v2(char *str, float a[2]);
+void print_v3(char *str, float a[3]);
+void print_v4(char *str, float a[4]);
+
+void normal_short_to_float_v3(float r[3], short n[3]);
+void normal_float_to_short_v3(short r[3], float n[3]);
+
+void minmax_v3_v3v3(float r[3], float min[3], float max[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BLI_MATH_VECTOR */
+
diff --git a/source/blender/blenlib/intern/math_base.c b/source/blender/blenlib/intern/math_base.c
new file mode 100644
index 00000000000..ca7b95cb3f1
--- /dev/null
+++ b/source/blender/blenlib/intern/math_base.c
@@ -0,0 +1,117 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "BLI_math.h"
+
+/* A few small defines. Keep'em local! */
+#define SMALL_NUMBER	1.e-8
+
+#if 0
+float sqrt3f(float f)
+{
+	if(f==0.0) return 0;
+	if(f<0) return (float)(-exp(log(-f)/3));
+	else return (float)(exp(log(f)/3));
+}
+#endif
+
+double sqrt3d(double d)
+{
+	if(d==0.0) return 0;
+	if(d<0) return -exp(log(-d)/3);
+	else return exp(log(d)/3);
+}
+
+#if 0
+float saacos(float fac)
+{
+	if(fac<= -1.0f) return (float)M_PI;
+	else if(fac>=1.0f) return 0.0;
+	else return (float)acos(fac);
+}
+
+float saasin(float fac)
+{
+	if(fac<= -1.0f) return (float)-M_PI/2.0f;
+	else if(fac>=1.0f) return (float)M_PI/2.0f;
+	else return (float)asin(fac);
+}
+
+float sasqrt(float fac)
+{
+	if(fac<=0.0) return 0.0;
+	return (float)sqrt(fac);
+}
+
+float saacosf(float fac)
+{
+	if(fac<= -1.0f) return (float)M_PI;
+	else if(fac>=1.0f) return 0.0f;
+	else return (float)acosf(fac);
+}
+
+float saasinf(float fac)
+{
+	if(fac<= -1.0f) return (float)-M_PI/2.0f;
+	else if(fac>=1.0f) return (float)M_PI/2.0f;
+	else return (float)asinf(fac);
+}
+
+float sasqrtf(float fac)
+{
+	if(fac<=0.0) return 0.0;
+	return (float)sqrtf(fac);
+}
+
+float interpf(float target, float origin, float fac)
+{
+	return (fac*target) + (1.0f-fac)*origin;
+}
+#endif
+
+/* useful to calculate an even width shell, by taking the angle between 2 planes.
+ * The return value is a scale on the offset.
+ * no angle between planes is 1.0, as the angle between the 2 planes approches 180d
+ * the distance gets very high, 180d would be inf, but this case isn't valid */
+float shell_angle_to_dist(const float angle)
+{
+	return (angle < SMALL_NUMBER) ? 1.0f : fabsf(1.0f / cosf(angle * (M_PI/180.0f)));
+}
+
+#if 0
+/* used for zoom values*/
+float power_of_2(float val)
+{
+	return (float)pow(2, ceil(log(val) / log(2)));
+}
+#endif
+
diff --git a/source/blender/blenlib/intern/math_color.c b/source/blender/blenlib/intern/math_color.c
new file mode 100644
index 00000000000..7ae380a1dde
--- /dev/null
+++ b/source/blender/blenlib/intern/math_color.c
@@ -0,0 +1,313 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ 
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "BLI_math.h"
+
+void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b)
+{
+	int i;
+	float f, p, q, t;
+
+	h *= 360.0f;
+	
+	if(s==0.0f) {
+		*r = v;
+		*g = v;
+		*b = v;
+	}
+	else {
+		if(h== 360.0f) h = 0.0f;
+		
+		h /= 60.0f;
+		i = (int)floor(h);
+		f = h - i;
+		p = v*(1.0f-s);
+		q = v*(1.0f-(s*f));
+		t = v*(1.0f-(s*(1.0f-f)));
+		
+		switch (i) {
+		case 0 :
+			*r = v;
+			*g = t;
+			*b = p;
+			break;
+		case 1 :
+			*r = q;
+			*g = v;
+			*b = p;
+			break;
+		case 2 :
+			*r = p;
+			*g = v;
+			*b = t;
+			break;
+		case 3 :
+			*r = p;
+			*g = q;
+			*b = v;
+			break;
+		case 4 :
+			*r = t;
+			*g = p;
+			*b = v;
+			break;
+		case 5 :
+			*r = v;
+			*g = p;
+			*b = q;
+			break;
+		}
+	}
+}
+
+void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv)
+{
+	float y, u, v;
+	y= 0.299f*r + 0.587f*g + 0.114f*b;
+	u=-0.147f*r - 0.289f*g + 0.436f*b;
+	v= 0.615f*r - 0.515f*g - 0.100f*b;
+	
+	*ly=y;
+	*lu=u;
+	*lv=v;
+}
+
+void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb)
+{
+	float r, g, b;
+	r=y+1.140f*v;
+	g=y-0.394f*u - 0.581f*v;
+	b=y+2.032f*u;
+	
+	*lr=r;
+	*lg=g;
+	*lb=b;
+}
+
+void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr)
+{
+	float sr,sg, sb;
+	float y, cr, cb;
+	
+	sr=255.0f*r;
+	sg=255.0f*g;
+	sb=255.0f*b;
+	
+	
+	y=(0.257f*sr)+(0.504f*sg)+(0.098f*sb)+16.0f;
+	cb=(-0.148f*sr)-(0.291f*sg)+(0.439f*sb)+128.0f;
+	cr=(0.439f*sr)-(0.368f*sg)-(0.071f*sb)+128.0f;
+	
+	*ly=y;
+	*lcb=cb;
+	*lcr=cr;
+}
+
+void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb)
+{
+	float r,g,b;
+	
+	r=1.164f*(y-16.0f)+1.596f*(cr-128.0f);
+	g=1.164f*(y-16.0f)-0.813f*(cr-128.0f)-0.392f*(cb-128.0f);
+	b=1.164f*(y-16.0f)+2.017f*(cb-128.0f);
+	
+	*lr=r/255.0f;
+	*lg=g/255.0f;
+	*lb=b/255.0f;
+}
+
+void hex_to_rgb(char *hexcol, float *r, float *g, float *b)
+{
+	unsigned int ri, gi, bi;
+	
+	if (hexcol[0] == '#') hexcol++;
+	
+	if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)) {
+		*r = ri / 255.0f;
+		*g = gi / 255.0f;		
+		*b = bi / 255.0f;
+	}
+}
+
+void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv)
+{
+	float h, s, v;
+	float cmax, cmin, cdelta;
+	float rc, gc, bc;
+
+	cmax = r;
+	cmin = r;
+	cmax = (g>cmax ? g:cmax);
+	cmin = (g<cmin ? g:cmin);
+	cmax = (b>cmax ? b:cmax);
+	cmin = (b<cmin ? b:cmin);
+
+	v = cmax;		/* value */
+	if (cmax != 0.0f)
+		s = (cmax - cmin)/cmax;
+	else {
+		s = 0.0f;
+		h = 0.0f;
+	}
+	if (s == 0.0f)
+		h = -1.0f;
+	else {
+		cdelta = cmax-cmin;
+		rc = (cmax-r)/cdelta;
+		gc = (cmax-g)/cdelta;
+		bc = (cmax-b)/cdelta;
+		if (r==cmax)
+			h = bc-gc;
+		else
+			if (g==cmax)
+				h = 2.0f+rc-bc;
+			else
+				h = 4.0f+gc-rc;
+		h = h*60.0f;
+		if (h < 0.0f)
+			h += 360.0f;
+	}
+	
+	*ls = s;
+	*lh = h / 360.0f;
+	if(*lh < 0.0f) *lh= 0.0f;
+	*lv = v;
+}
+
+/*http://brucelindbloom.com/index.html?Eqn_RGB_XYZ_Matrix.html */
+
+void xyz_to_rgb(float xc, float yc, float zc, float *r, float *g, float *b, int colorspace)
+{
+	switch (colorspace) { 
+	case BLI_CS_SMPTE:
+		*r = (3.50570f	 * xc) + (-1.73964f	 * yc) + (-0.544011f * zc);
+		*g = (-1.06906f	 * xc) + (1.97781f	 * yc) + (0.0351720f * zc);
+		*b = (0.0563117f * xc) + (-0.196994f * yc) + (1.05005f	 * zc);
+		break;
+	case BLI_CS_REC709:
+		*r = (3.240476f	 * xc) + (-1.537150f * yc) + (-0.498535f * zc);
+		*g = (-0.969256f * xc) + (1.875992f  * yc) + (0.041556f  * zc);
+		*b = (0.055648f	 * xc) + (-0.204043f * yc) + (1.057311f  * zc);
+		break;
+	case BLI_CS_CIE:
+		*r = (2.28783848734076f	* xc) + (-0.833367677835217f	* yc) + (-0.454470795871421f	* zc);
+		*g = (-0.511651380743862f * xc) + (1.42275837632178f * yc) + (0.0888930017552939f * zc);
+		*b = (0.00572040983140966f	* xc) + (-0.0159068485104036f	* yc) + (1.0101864083734f	* zc);
+		break;
+	}
+}
+
+/* we define a 'cpack' here as a (3 byte color code) number that can be expressed like 0xFFAA66 or so.
+   for that reason it is sensitive for endianness... with this function it works correctly
+*/
+
+unsigned int hsv_to_cpack(float h, float s, float v)
+{
+	short r, g, b;
+	float rf, gf, bf;
+	unsigned int col;
+	
+	hsv_to_rgb(h, s, v, &rf, &gf, &bf);
+	
+	r= (short)(rf*255.0f);
+	g= (short)(gf*255.0f);
+	b= (short)(bf*255.0f);
+	
+	col= ( r + (g*256) + (b*256*256) );
+	return col;
+}
+
+
+unsigned int rgb_to_cpack(float r, float g, float b)
+{
+	int ir, ig, ib;
+	
+	ir= (int)floor(255.0*r);
+	if(ir<0) ir= 0; else if(ir>255) ir= 255;
+	ig= (int)floor(255.0*g);
+	if(ig<0) ig= 0; else if(ig>255) ig= 255;
+	ib= (int)floor(255.0*b);
+	if(ib<0) ib= 0; else if(ib>255) ib= 255;
+	
+	return (ir+ (ig*256) + (ib*256*256));
+}
+
+void cpack_to_rgb(unsigned int col, float *r, float *g, float *b)
+{
+	
+	*r= (float)((col)&0xFF);
+	*r /= 255.0f;
+
+	*g= (float)(((col)>>8)&0xFF);
+	*g /= 255.0f;
+
+	*b= (float)(((col)>>16)&0xFF);
+	*b /= 255.0f;
+}
+
+void minmax_rgb(short c[])
+{
+	if(c[0]>255) c[0]=255;
+	else if(c[0]<0) c[0]=0;
+	if(c[1]>255) c[1]=255;
+	else if(c[1]<0) c[1]=0;
+	if(c[2]>255) c[2]=255;
+	else if(c[2]<0) c[2]=0;
+}
+
+/*If the requested RGB shade contains a negative weight for
+  one of the primaries, it lies outside the colour gamut 
+  accessible from the given triple of primaries.  Desaturate
+  it by adding white, equal quantities of R, G, and B, enough
+  to make RGB all positive.  The function returns 1 if the
+  components were modified, zero otherwise.*/
+int constrain_rgb(float *r, float *g, float *b)
+{
+	float w;
+
+    /* Amount of white needed is w = - min(0, *r, *g, *b) */
+    
+    w = (0 < *r) ? 0 : *r;
+    w = (w < *g) ? w : *g;
+    w = (w < *b) ? w : *b;
+    w = -w;
+
+    /* Add just enough white to make r, g, b all positive. */
+    
+    if (w > 0) {
+        *r += w;  *g += w; *b += w;
+        return 1;                     /* Color modified to fit RGB gamut */
+    }
+
+    return 0;                         /* Color within RGB gamut */
+}
+
diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c
new file mode 100644
index 00000000000..5c7890ffee4
--- /dev/null
+++ b/source/blender/blenlib/intern/math_geom.c
@@ -0,0 +1,1598 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ 
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "BLI_math.h"
+#include "BLI_memarena.h"
+
+/********************************** Polygons *********************************/
+
+void cent_tri_v3(float *cent, float *v1, float *v2, float *v3)
+{
+	cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]);
+	cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]);
+	cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]);
+}
+
+void cent_quad_v3(float *cent, float *v1, float *v2, float *v3, float *v4)
+{
+	cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]);
+	cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]);
+	cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]);
+}
+
+float normal_tri_v3(float *n, float *v1, float *v2, float *v3)
+{
+	float n1[3],n2[3];
+
+	n1[0]= v1[0]-v2[0];
+	n2[0]= v2[0]-v3[0];
+	n1[1]= v1[1]-v2[1];
+	n2[1]= v2[1]-v3[1];
+	n1[2]= v1[2]-v2[2];
+	n2[2]= v2[2]-v3[2];
+	n[0]= n1[1]*n2[2]-n1[2]*n2[1];
+	n[1]= n1[2]*n2[0]-n1[0]*n2[2];
+	n[2]= n1[0]*n2[1]-n1[1]*n2[0];
+	return normalize_v3(n);
+}
+
+float normal_quad_v3(float *n, float *v1, float *v2, float *v3, float *v4)
+{
+	/* real cross! */
+	float n1[3],n2[3];
+
+	n1[0]= v1[0]-v3[0];
+	n1[1]= v1[1]-v3[1];
+	n1[2]= v1[2]-v3[2];
+
+	n2[0]= v2[0]-v4[0];
+	n2[1]= v2[1]-v4[1];
+	n2[2]= v2[2]-v4[2];
+
+	n[0]= n1[1]*n2[2]-n1[2]*n2[1];
+	n[1]= n1[2]*n2[0]-n1[0]*n2[2];
+	n[2]= n1[0]*n2[1]-n1[1]*n2[0];
+
+	return normalize_v3(n);
+}
+
+float area_tri_v2(float *v1, float *v2, float *v3)
+{
+	return (float)(0.5*fabs((v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0])));
+}
+
+
+float area_quad_v3(float *v1, float *v2, float *v3,  float *v4)  /* only convex Quadrilaterals */
+{
+	float len, vec1[3], vec2[3], n[3];
+
+	sub_v3_v3v3(vec1, v2, v1);
+	sub_v3_v3v3(vec2, v4, v1);
+	cross_v3_v3v3(n, vec1, vec2);
+	len= normalize_v3(n);
+
+	sub_v3_v3v3(vec1, v4, v3);
+	sub_v3_v3v3(vec2, v2, v3);
+	cross_v3_v3v3(n, vec1, vec2);
+	len+= normalize_v3(n);
+
+	return (len/2.0f);
+}
+
+float area_tri_v3(float *v1, float *v2, float *v3)  /* Triangles */
+{
+	float len, vec1[3], vec2[3], n[3];
+
+	sub_v3_v3v3(vec1, v3, v2);
+	sub_v3_v3v3(vec2, v1, v2);
+	cross_v3_v3v3(n, vec1, vec2);
+	len= normalize_v3(n);
+
+	return (len/2.0f);
+}
+
+#define MAX2(x,y)		((x)>(y) ? (x) : (y))
+#define MAX3(x,y,z)		MAX2(MAX2((x),(y)) , (z))
+
+
+float area_poly_v3(int nr, float *verts, float *normal)
+{
+	float x, y, z, area, max;
+	float *cur, *prev;
+	int a, px=0, py=1;
+
+	/* first: find dominant axis: 0==X, 1==Y, 2==Z */
+	x= (float)fabs(normal[0]);
+	y= (float)fabs(normal[1]);
+	z= (float)fabs(normal[2]);
+	max = MAX3(x, y, z);
+	if(max==y) py=2;
+	else if(max==x) {
+		px=1; 
+		py= 2;
+	}
+
+	/* The Trapezium Area Rule */
+	prev= verts+3*(nr-1);
+	cur= verts;
+	area= 0;
+	for(a=0; a<nr; a++) {
+		area+= (cur[px]-prev[px])*(cur[py]+prev[py]);
+		prev= cur;
+		cur+=3;
+	}
+
+	return (float)fabs(0.5*area/max);
+}
+
+/********************************* Distance **********************************/
+
+/* distance v1 to line v2-v3 */
+/* using Hesse formula, NO LINE PIECE! */
+float dist_to_line_v2(float *v1, float *v2, float *v3)
+{
+	float a[2],deler;
+
+	a[0]= v2[1]-v3[1];
+	a[1]= v3[0]-v2[0];
+	deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]);
+	if(deler== 0.0f) return 0;
+
+	return (float)(fabs((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler);
+
+}
+
+/* distance v1 to line-piece v2-v3 */
+float dist_to_line_segment_v2(float *v1, float *v2, float *v3) 
+{
+	float labda, rc[2], pt[2], len;
+	
+	rc[0]= v3[0]-v2[0];
+	rc[1]= v3[1]-v2[1];
+	len= rc[0]*rc[0]+ rc[1]*rc[1];
+	if(len==0.0) {
+		rc[0]= v1[0]-v2[0];
+		rc[1]= v1[1]-v2[1];
+		return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1]));
+	}
+	
+	labda= (rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]))/len;
+	if(labda<=0.0) {
+		pt[0]= v2[0];
+		pt[1]= v2[1];
+	}
+	else if(labda>=1.0) {
+		pt[0]= v3[0];
+		pt[1]= v3[1];
+	}
+	else {
+		pt[0]= labda*rc[0]+v2[0];
+		pt[1]= labda*rc[1]+v2[1];
+	}
+
+	rc[0]= pt[0]-v1[0];
+	rc[1]= pt[1]-v1[1];
+	return (float)sqrt(rc[0]*rc[0]+ rc[1]*rc[1]);
+}
+
+/* point closest to v1 on line v2-v3 in 3D */
+void closest_to_line_segment_v3(float *closest, float v1[3], float v2[3], float v3[3])
+{
+	float lambda, cp[3];
+
+	lambda= closest_to_line_v3(cp,v1, v2, v3);
+
+	if(lambda <= 0.0f)
+		copy_v3_v3(closest, v2);
+	else if(lambda >= 1.0f)
+		copy_v3_v3(closest, v3);
+	else
+		copy_v3_v3(closest, cp);
+}
+
+/* distance v1 to line-piece v2-v3 in 3D */
+float dist_to_line_segment_v3(float *v1, float *v2, float *v3) 
+{
+	float closest[3];
+
+	closest_to_line_segment_v3(closest, v1, v2, v3);
+
+	return len_v3v3(closest, v1);
+}
+
+/******************************* Intersection ********************************/
+
+/* intersect Line-Line, shorts */
+short isect_line_line_v2_short(short *v1, short *v2, short *v3, short *v4)
+{
+	/* return:
+	-1: colliniar
+	 0: no intersection of segments
+	 1: exact intersection of segments
+	 2: cross-intersection of segments
+	*/
+	float div, labda, mu;
+	
+	div= (float)((v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]));
+	if(div==0.0f) return -1;
+	
+	labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
+	
+	mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
+	
+	if(labda>=0.0f && labda<=1.0f && mu>=0.0f && mu<=1.0f) {
+		if(labda==0.0f || labda==1.0f || mu==0.0f || mu==1.0f) return 1;
+		return 2;
+	}
+	return 0;
+}
+
+/* intersect Line-Line, floats */
+short isect_line_line_v2(float *v1, float *v2, float *v3, float *v4)
+{
+	/* return:
+	-1: colliniar
+0: no intersection of segments
+1: exact intersection of segments
+2: cross-intersection of segments
+	*/
+	float div, labda, mu;
+	
+	div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]);
+	if(div==0.0) return -1;
+	
+	labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div;
+	
+	mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div;
+	
+	if(labda>=0.0 && labda<=1.0 && mu>=0.0 && mu<=1.0) {
+		if(labda==0.0 || labda==1.0 || mu==0.0 || mu==1.0) return 1;
+		return 2;
+	}
+	return 0;
+}
+
+/*
+-1: colliniar
+ 1: intersection
+
+*/
+static short IsectLLPt2Df(float x0,float y0,float x1,float y1,
+					 float x2,float y2,float x3,float y3, float *xi,float *yi)
+
+{
+	/*
+	this function computes the intersection of the sent lines
+	and returns the intersection point, note that the function assumes
+	the lines intersect. the function can handle vertical as well
+	as horizontal lines. note the function isn't very clever, it simply
+	applies the math, but we don't need speed since this is a
+	pre-processing step
+	*/
+	float c1,c2, // constants of linear equations
+	det_inv,  // the inverse of the determinant of the coefficient
+	m1,m2;    // the slopes of each line
+	/*
+	compute slopes, note the cludge for infinity, however, this will
+	be close enough
+	*/
+	if (fabs(x1-x0) > 0.000001)
+		m1 = (y1-y0) / (x1-x0);
+	else
+		return -1; /*m1 = (float) 1e+10;*/   // close enough to infinity
+
+	if (fabs(x3-x2) > 0.000001)
+		m2 = (y3-y2) / (x3-x2);
+	else
+		return -1; /*m2 = (float) 1e+10;*/   // close enough to infinity
+
+	if (fabs(m1-m2) < 0.000001)
+		return -1; /* paralelle lines */
+	
+// compute constants
+
+	c1 = (y0-m1*x0);
+	c2 = (y2-m2*x2);
+
+// compute the inverse of the determinate
+
+	det_inv = 1.0f / (-m1 + m2);
+
+// use Kramers rule to compute xi and yi
+
+	*xi= ((-c2 + c1) *det_inv);
+	*yi= ((m2*c1 - m1*c2) *det_inv);
+	
+	return 1; 
+} // end Intersect_Lines
+
+#define SIDE_OF_LINE(pa,pb,pp)	((pa[0]-pp[0])*(pb[1]-pp[1]))-((pb[0]-pp[0])*(pa[1]-pp[1]))
+/* point in tri */
+int isect_point_tri_v2(float pt[2], float v1[2], float v2[2], float v3[2])
+{
+	if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
+		if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
+			if (SIDE_OF_LINE(v3,v1,pt)>=0.0) {
+				return 1;
+			}
+		}
+	} else {
+		if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0)) {
+			if (! (SIDE_OF_LINE(v3,v1,pt)>=0.0)) {
+				return -1;
+			}
+		}
+	}
+	
+	return 0;
+}
+/* point in quad - only convex quads */
+int isect_point_quad_v2(float pt[2], float v1[2], float v2[2], float v3[2], float v4[2])
+{
+	if (SIDE_OF_LINE(v1,v2,pt)>=0.0) {
+		if (SIDE_OF_LINE(v2,v3,pt)>=0.0) {
+			if (SIDE_OF_LINE(v3,v4,pt)>=0.0) {
+				if (SIDE_OF_LINE(v4,v1,pt)>=0.0) {
+					return 1;
+				}
+			}
+		}
+	} else {
+		if (! (SIDE_OF_LINE(v2,v3,pt)>=0.0)) {
+			if (! (SIDE_OF_LINE(v3,v4,pt)>=0.0)) {
+				if (! (SIDE_OF_LINE(v4,v1,pt)>=0.0)) {
+					return -1;
+				}
+			}
+		}
+	}
+	
+	return 0;
+}
+
+/* moved from effect.c
+   test if the line starting at p1 ending at p2 intersects the triangle v0..v2
+   return non zero if it does 
+*/
+int isect_line_tri_v3(float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda, float *uv)
+{
+
+	float p[3], s[3], d[3], e1[3], e2[3], q[3];
+	float a, f, u, v;
+	
+	sub_v3_v3v3(e1, v1, v0);
+	sub_v3_v3v3(e2, v2, v0);
+	sub_v3_v3v3(d, p2, p1);
+	
+	cross_v3_v3v3(p, d, e2);
+	a = dot_v3v3(e1, p);
+	if ((a > -0.000001) && (a < 0.000001)) return 0;
+	f = 1.0f/a;
+	
+	sub_v3_v3v3(s, p1, v0);
+	
+	cross_v3_v3v3(q, s, e1);
+	*lambda = f * dot_v3v3(e2, q);
+	if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
+	
+	u = f * dot_v3v3(s, p);
+	if ((u < 0.0)||(u > 1.0)) return 0;
+	
+	v = f * dot_v3v3(d, q);
+	if ((v < 0.0)||((u + v) > 1.0)) return 0;
+
+	if(uv) {
+		uv[0]= u;
+		uv[1]= v;
+	}
+	
+	return 1;
+}
+
+/* moved from effect.c
+   test if the ray starting at p1 going in d direction intersects the triangle v0..v2
+   return non zero if it does 
+*/
+int isect_ray_tri_v3(float p1[3], float d[3], float v0[3], float v1[3], float v2[3], float *lambda, float *uv)
+{
+	float p[3], s[3], e1[3], e2[3], q[3];
+	float a, f, u, v;
+	
+	sub_v3_v3v3(e1, v1, v0);
+	sub_v3_v3v3(e2, v2, v0);
+	
+	cross_v3_v3v3(p, d, e2);
+	a = dot_v3v3(e1, p);
+	if ((a > -0.000001) && (a < 0.000001)) return 0;
+	f = 1.0f/a;
+	
+	sub_v3_v3v3(s, p1, v0);
+	
+	cross_v3_v3v3(q, s, e1);
+	*lambda = f * dot_v3v3(e2, q);
+	if ((*lambda < 0.0)) return 0;
+	
+	u = f * dot_v3v3(s, p);
+	if ((u < 0.0)||(u > 1.0)) return 0;
+	
+	v = f * dot_v3v3(d, q);
+	if ((v < 0.0)||((u + v) > 1.0)) return 0;
+
+	if(uv) {
+		uv[0]= u;
+		uv[1]= v;
+	}
+	
+	return 1;
+}
+
+int isect_ray_tri_threshold_v3(float p1[3], float d[3], float v0[3], float v1[3], float v2[3], float *lambda, float *uv, float threshold)
+{
+	float p[3], s[3], e1[3], e2[3], q[3];
+	float a, f, u, v;
+	float du = 0, dv = 0;
+	
+	sub_v3_v3v3(e1, v1, v0);
+	sub_v3_v3v3(e2, v2, v0);
+	
+	cross_v3_v3v3(p, d, e2);
+	a = dot_v3v3(e1, p);
+	if ((a > -0.000001) && (a < 0.000001)) return 0;
+	f = 1.0f/a;
+	
+	sub_v3_v3v3(s, p1, v0);
+	
+	cross_v3_v3v3(q, s, e1);
+	*lambda = f * dot_v3v3(e2, q);
+	if ((*lambda < 0.0)) return 0;
+	
+	u = f * dot_v3v3(s, p);
+	v = f * dot_v3v3(d, q);
+	
+	if (u < 0) du = u;
+	if (u > 1) du = u - 1;
+	if (v < 0) dv = v;
+	if (v > 1) dv = v - 1;
+	if (u > 0 && v > 0 && u + v > 1)
+	{
+		float t = u + v - 1;
+		du = u - t/2;
+		dv = v - t/2;
+	}
+
+	mul_v3_fl(e1, du);
+	mul_v3_fl(e2, dv);
+	
+	if (dot_v3v3(e1, e1) + dot_v3v3(e2, e2) > threshold * threshold)
+	{
+		return 0;
+	}
+
+	if(uv) {
+		uv[0]= u;
+		uv[1]= v;
+	}
+	
+	return 1;
+}
+
+
+/* Adapted from the paper by Kasper Fauerby */
+/* "Improved Collision detection and Response" */
+static int getLowestRoot(float a, float b, float c, float maxR, float* root)
+{
+	// Check if a solution exists
+	float determinant = b*b - 4.0f*a*c;
+
+	// If determinant is negative it means no solutions.
+	if (determinant >= 0.0f)
+	{
+		// calculate the two roots: (if determinant == 0 then
+		// x1==x2 but let’s disregard that slight optimization)
+		float sqrtD = (float)sqrt(determinant);
+		float r1 = (-b - sqrtD) / (2.0f*a);
+		float r2 = (-b + sqrtD) / (2.0f*a);
+		
+		// Sort so x1 <= x2
+		if (r1 > r2)
+			SWAP(float, r1, r2);
+
+		// Get lowest root:
+		if (r1 > 0.0f && r1 < maxR)
+		{
+			*root = r1;
+			return 1;
+		}
+
+		// It is possible that we want x2 - this can happen
+		// if x1 < 0
+		if (r2 > 0.0f && r2 < maxR)
+		{
+			*root = r2;
+			return 1;
+		}
+	}
+	// No (valid) solutions
+	return 0;
+}
+
+int isect_sweeping_sphere_tri_v3(float p1[3], float p2[3], float radius, float v0[3], float v1[3], float v2[3], float *lambda, float *ipoint)
+{
+	float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3];
+	float a, b, c, d, e, x, y, z, radius2=radius*radius;
+	float elen2,edotv,edotbv,nordotv,vel2;
+	float newLambda;
+	int found_by_sweep=0;
+
+	sub_v3_v3v3(e1,v1,v0);
+	sub_v3_v3v3(e2,v2,v0);
+	sub_v3_v3v3(vel,p2,p1);
+
+/*---test plane of tri---*/
+	cross_v3_v3v3(nor,e1,e2);
+	normalize_v3(nor);
+
+	/* flip normal */
+	if(dot_v3v3(nor,vel)>0.0f) negate_v3(nor);
+	
+	a=dot_v3v3(p1,nor)-dot_v3v3(v0,nor);
+	nordotv=dot_v3v3(nor,vel);
+
+	if (fabs(nordotv) < 0.000001)
+	{
+		if(fabs(a)>=radius)
+		{
+			return 0;
+		}
+	}
+	else
+	{
+		float t0=(-a+radius)/nordotv;
+		float t1=(-a-radius)/nordotv;
+
+		if(t0>t1)
+			SWAP(float, t0, t1);
+
+		if(t0>1.0f || t1<0.0f) return 0;
+
+		/* clamp to [0,1] */
+		CLAMP(t0, 0.0f, 1.0f);
+		CLAMP(t1, 0.0f, 1.0f);
+
+		/*---test inside of tri---*/
+		/* plane intersection point */
+
+		point[0] = p1[0] + vel[0]*t0 - nor[0]*radius;
+		point[1] = p1[1] + vel[1]*t0 - nor[1]*radius;
+		point[2] = p1[2] + vel[2]*t0 - nor[2]*radius;
+
+
+		/* is the point in the tri? */
+		a=dot_v3v3(e1,e1);
+		b=dot_v3v3(e1,e2);
+		c=dot_v3v3(e2,e2);
+
+		sub_v3_v3v3(temp,point,v0);
+		d=dot_v3v3(temp,e1);
+		e=dot_v3v3(temp,e2);
+		
+		x=d*c-e*b;
+		y=e*a-d*b;
+		z=x+y-(a*c-b*b);
+
+
+		if(z <= 0.0f && (x >= 0.0f && y >= 0.0f))
+		{
+		//(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000){
+			*lambda=t0;
+			copy_v3_v3(ipoint,point);
+			return 1;
+		}
+	}
+
+
+	*lambda=1.0f;
+
+/*---test points---*/
+	a=vel2=dot_v3v3(vel,vel);
+
+	/*v0*/
+	sub_v3_v3v3(temp,p1,v0);
+	b=2.0f*dot_v3v3(vel,temp);
+	c=dot_v3v3(temp,temp)-radius2;
+
+	if(getLowestRoot(a, b, c, *lambda, lambda))
+	{
+		copy_v3_v3(ipoint,v0);
+		found_by_sweep=1;
+	}
+
+	/*v1*/
+	sub_v3_v3v3(temp,p1,v1);
+	b=2.0f*dot_v3v3(vel,temp);
+	c=dot_v3v3(temp,temp)-radius2;
+
+	if(getLowestRoot(a, b, c, *lambda, lambda))
+	{
+		copy_v3_v3(ipoint,v1);
+		found_by_sweep=1;
+	}
+	
+	/*v2*/
+	sub_v3_v3v3(temp,p1,v2);
+	b=2.0f*dot_v3v3(vel,temp);
+	c=dot_v3v3(temp,temp)-radius2;
+
+	if(getLowestRoot(a, b, c, *lambda, lambda))
+	{
+		copy_v3_v3(ipoint,v2);
+		found_by_sweep=1;
+	}
+
+/*---test edges---*/
+	sub_v3_v3v3(e3,v2,v1); //wasnt yet calculated
+
+
+	/*e1*/
+	sub_v3_v3v3(bv,v0,p1);
+
+	elen2 = dot_v3v3(e1,e1);
+	edotv = dot_v3v3(e1,vel);
+	edotbv = dot_v3v3(e1,bv);
+
+	a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
+	b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
+	c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
+
+	if(getLowestRoot(a, b, c, *lambda, &newLambda))
+	{
+		e=(edotv*newLambda-edotbv)/elen2;
+
+		if(e >= 0.0f && e <= 1.0f)
+		{
+			*lambda = newLambda;
+			copy_v3_v3(ipoint,e1);
+			mul_v3_fl(ipoint,e);
+			add_v3_v3v3(ipoint,ipoint,v0);
+			found_by_sweep=1;
+		}
+	}
+
+	/*e2*/
+	/*bv is same*/
+	elen2 = dot_v3v3(e2,e2);
+	edotv = dot_v3v3(e2,vel);
+	edotbv = dot_v3v3(e2,bv);
+
+	a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
+	b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
+	c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
+
+	if(getLowestRoot(a, b, c, *lambda, &newLambda))
+	{
+		e=(edotv*newLambda-edotbv)/elen2;
+
+		if(e >= 0.0f && e <= 1.0f)
+		{
+			*lambda = newLambda;
+			copy_v3_v3(ipoint,e2);
+			mul_v3_fl(ipoint,e);
+			add_v3_v3v3(ipoint,ipoint,v0);
+			found_by_sweep=1;
+		}
+	}
+
+	/*e3*/
+	sub_v3_v3v3(bv,v0,p1);
+	elen2 = dot_v3v3(e1,e1);
+	edotv = dot_v3v3(e1,vel);
+	edotbv = dot_v3v3(e1,bv);
+
+	sub_v3_v3v3(bv,v1,p1);
+	elen2 = dot_v3v3(e3,e3);
+	edotv = dot_v3v3(e3,vel);
+	edotbv = dot_v3v3(e3,bv);
+
+	a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv;
+	b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv);
+	c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv;
+
+	if(getLowestRoot(a, b, c, *lambda, &newLambda))
+	{
+		e=(edotv*newLambda-edotbv)/elen2;
+
+		if(e >= 0.0f && e <= 1.0f)
+		{
+			*lambda = newLambda;
+			copy_v3_v3(ipoint,e3);
+			mul_v3_fl(ipoint,e);
+			add_v3_v3v3(ipoint,ipoint,v1);
+			found_by_sweep=1;
+		}
+	}
+
+
+	return found_by_sweep;
+}
+int isect_axial_line_tri_v3(int axis, float p1[3], float p2[3], float v0[3], float v1[3], float v2[3], float *lambda)
+{
+	float p[3], e1[3], e2[3];
+	float u, v, f;
+	int a0=axis, a1=(axis+1)%3, a2=(axis+2)%3;
+
+	//return isect_line_tri_v3(p1,p2,v0,v1,v2,lambda);
+
+	///* first a simple bounding box test */
+	//if(MIN3(v0[a1],v1[a1],v2[a1]) > p1[a1]) return 0;
+	//if(MIN3(v0[a2],v1[a2],v2[a2]) > p1[a2]) return 0;
+	//if(MAX3(v0[a1],v1[a1],v2[a1]) < p1[a1]) return 0;
+	//if(MAX3(v0[a2],v1[a2],v2[a2]) < p1[a2]) return 0;
+
+	///* then a full intersection test */
+	
+	sub_v3_v3v3(e1,v1,v0);
+	sub_v3_v3v3(e2,v2,v0);
+	sub_v3_v3v3(p,v0,p1);
+
+	f= (e2[a1]*e1[a2]-e2[a2]*e1[a1]);
+	if ((f > -0.000001) && (f < 0.000001)) return 0;
+
+	v= (p[a2]*e1[a1]-p[a1]*e1[a2])/f;
+	if ((v < 0.0)||(v > 1.0)) return 0;
+	
+	f= e1[a1];
+	if((f > -0.000001) && (f < 0.000001)){
+		f= e1[a2];
+		if((f > -0.000001) && (f < 0.000001)) return 0;
+		u= (-p[a2]-v*e2[a2])/f;
+	}
+	else
+		u= (-p[a1]-v*e2[a1])/f;
+
+	if ((u < 0.0)||((u + v) > 1.0)) return 0;
+
+	*lambda = (p[a0]+u*e1[a0]+v*e2[a0])/(p2[a0]-p1[a0]);
+
+	if ((*lambda < 0.0)||(*lambda > 1.0)) return 0;
+
+	return 1;
+}
+
+/* Returns the number of point of interests
+ * 0 - lines are colinear
+ * 1 - lines are coplanar, i1 is set to intersection
+ * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively 
+ * */
+int isect_line_line_v3(float v1[3], float v2[3], float v3[3], float v4[3], float i1[3], float i2[3])
+{
+	float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3];
+	float d;
+	
+	sub_v3_v3v3(c, v3, v1);
+	sub_v3_v3v3(a, v2, v1);
+	sub_v3_v3v3(b, v4, v3);
+
+	copy_v3_v3(dir1, a);
+	normalize_v3(dir1);
+	copy_v3_v3(dir2, b);
+	normalize_v3(dir2);
+	d = dot_v3v3(dir1, dir2);
+	if (d == 1.0f || d == -1.0f) {
+		/* colinear */
+		return 0;
+	}
+
+	cross_v3_v3v3(ab, a, b);
+	d = dot_v3v3(c, ab);
+
+	/* test if the two lines are coplanar */
+	if (d > -0.000001f && d < 0.000001f) {
+		cross_v3_v3v3(cb, c, b);
+
+		mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
+		add_v3_v3v3(i1, v1, a);
+		copy_v3_v3(i2, i1);
+		
+		return 1; /* one intersection only */
+	}
+	/* if not */
+	else {
+		float n[3], t[3];
+		float v3t[3], v4t[3];
+		sub_v3_v3v3(t, v1, v3);
+
+		/* offset between both plane where the lines lies */
+		cross_v3_v3v3(n, a, b);
+		project_v3_v3v3(t, t, n);
+
+		/* for the first line, offset the second line until it is coplanar */
+		add_v3_v3v3(v3t, v3, t);
+		add_v3_v3v3(v4t, v4, t);
+		
+		sub_v3_v3v3(c, v3t, v1);
+		sub_v3_v3v3(a, v2, v1);
+		sub_v3_v3v3(b, v4t, v3t);
+
+		cross_v3_v3v3(ab, a, b);
+		cross_v3_v3v3(cb, c, b);
+
+		mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab));
+		add_v3_v3v3(i1, v1, a);
+
+		/* for the second line, just substract the offset from the first intersection point */
+		sub_v3_v3v3(i2, i1, t);
+		
+		return 2; /* two nearest points */
+	}
+} 
+
+/* Intersection point strictly between the two lines
+ * 0 when no intersection is found 
+ * */
+int isect_line_line_strict_v3(float v1[3], float v2[3], float v3[3], float v4[3], float vi[3], float *lambda)
+{
+	float a[3], b[3], c[3], ab[3], cb[3], ca[3], dir1[3], dir2[3];
+	float d;
+	float d1;
+	
+	sub_v3_v3v3(c, v3, v1);
+	sub_v3_v3v3(a, v2, v1);
+	sub_v3_v3v3(b, v4, v3);
+
+	copy_v3_v3(dir1, a);
+	normalize_v3(dir1);
+	copy_v3_v3(dir2, b);
+	normalize_v3(dir2);
+	d = dot_v3v3(dir1, dir2);
+	if (d == 1.0f || d == -1.0f || d == 0) {
+		/* colinear or one vector is zero-length*/
+		return 0;
+	}
+	
+	d1 = d;
+
+	cross_v3_v3v3(ab, a, b);
+	d = dot_v3v3(c, ab);
+
+	/* test if the two lines are coplanar */
+	if (d > -0.000001f && d < 0.000001f) {
+		float f1, f2;
+		cross_v3_v3v3(cb, c, b);
+		cross_v3_v3v3(ca, c, a);
+
+		f1 = dot_v3v3(cb, ab) / dot_v3v3(ab, ab);
+		f2 = dot_v3v3(ca, ab) / dot_v3v3(ab, ab);
+		
+		if (f1 >= 0 && f1 <= 1 &&
+			f2 >= 0 && f2 <= 1)
+		{
+			mul_v3_fl(a, f1);
+			add_v3_v3v3(vi, v1, a);
+			
+			if (lambda != NULL)
+			{
+				*lambda = f1;
+			}
+			
+			return 1; /* intersection found */
+		}
+		else
+		{
+			return 0;
+		}
+	}
+	else
+	{
+		return 0;
+	}
+} 
+
+int isect_aabb_aabb_v3(float min1[3], float max1[3], float min2[3], float max2[3])
+{
+	return (min1[0]<max2[0] && min1[1]<max2[1] && min1[2]<max2[2] &&
+	        min2[0]<max1[0] && min2[1]<max1[1] && min2[2]<max1[2]);
+}
+
+/* find closest point to p on line through l1,l2 and return lambda,
+ * where (0 <= lambda <= 1) when cp is in the line segement l1,l2
+ */
+float closest_to_line_v3(float cp[3],float p[3], float l1[3], float l2[3])
+{
+	float h[3],u[3],lambda;
+	sub_v3_v3v3(u, l2, l1);
+	sub_v3_v3v3(h, p, l1);
+	lambda =dot_v3v3(u,h)/dot_v3v3(u,u);
+	cp[0] = l1[0] + u[0] * lambda;
+	cp[1] = l1[1] + u[1] * lambda;
+	cp[2] = l1[2] + u[2] * lambda;
+	return lambda;
+}
+
+#if 0
+/* little sister we only need to know lambda */
+static float lambda_cp_line(float p[3], float l1[3], float l2[3])
+{
+	float h[3],u[3];
+	sub_v3_v3v3(u, l2, l1);
+	sub_v3_v3v3(h, p, l1);
+	return(dot_v3v3(u,h)/dot_v3v3(u,u));
+}
+#endif
+
+/* Similar to LineIntersectsTriangleUV, except it operates on a quad and in 2d, assumes point is in quad */
+void isect_point_quad_uv_v2(float v0[2], float v1[2], float v2[2], float v3[2], float pt[2], float *uv)
+{
+	float x0,y0, x1,y1, wtot, v2d[2], w1, w2;
+	
+	/* used for paralelle lines */
+	float pt3d[3], l1[3], l2[3], pt_on_line[3];
+	
+	/* compute 2 edges  of the quad  intersection point */
+	if (IsectLLPt2Df(v0[0],v0[1],v1[0],v1[1],  v2[0],v2[1],v3[0],v3[1], &x0,&y0) == 1) {
+		/* the intersection point between the quad-edge intersection and the point in the quad we want the uv's for */
+		/* should never be paralle !! */
+		/*printf("\tnot paralelle 1\n");*/
+		IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v3[0],v3[1], &x1,&y1);
+		
+		/* Get the weights from the new intersection point, to each edge */
+		v2d[0] = x1-v0[0];
+		v2d[1] = y1-v0[1];
+		w1 = len_v2(v2d);
+		
+		v2d[0] = x1-v3[0]; /* some but for the other vert */
+		v2d[1] = y1-v3[1];
+		w2 = len_v2(v2d);
+		wtot = w1+w2;
+		/*w1 = w1/wtot;*/
+		/*w2 = w2/wtot;*/
+		uv[0] = w1/wtot;
+	} else {
+		/* lines are paralelle, lambda_cp_line_ex is 3d grrr */
+		/*printf("\tparalelle1\n");*/
+		pt3d[0] = pt[0];
+		pt3d[1] = pt[1];
+		pt3d[2] = l1[2] = l2[2] = 0.0f;
+		
+		l1[0] = v0[0]; l1[1] = v0[1];
+		l2[0] = v1[0]; l2[1] = v1[1];
+		closest_to_line_v3(pt_on_line,pt3d, l1, l2);
+		v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
+		v2d[1] = pt[1]-pt_on_line[1];
+		w1 = len_v2(v2d);
+		
+		l1[0] = v2[0]; l1[1] = v2[1];
+		l2[0] = v3[0]; l2[1] = v3[1];
+		closest_to_line_v3(pt_on_line,pt3d, l1, l2);
+		v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */
+		v2d[1] = pt[1]-pt_on_line[1];
+		w2 = len_v2(v2d);
+		wtot = w1+w2;
+		uv[0] = w1/wtot;	
+	}
+	
+	/* Same as above to calc the uv[1] value, alternate calculation */
+	
+	if (IsectLLPt2Df(v0[0],v0[1],v3[0],v3[1],  v1[0],v1[1],v2[0],v2[1], &x0,&y0) == 1) { /* was v0,v1  v2,v3  now v0,v3  v1,v2*/
+		/* never paralle if above was not */
+		/*printf("\tnot paralelle2\n");*/
+		IsectLLPt2Df(pt[0],pt[1],x0,y0,  v0[0],v0[1],v1[0],v1[1], &x1,&y1);/* was v0,v3  now v0,v1*/
+		
+		v2d[0] = x1-v0[0];
+		v2d[1] = y1-v0[1];
+		w1 = len_v2(v2d);
+		
+		v2d[0] = x1-v1[0];
+		v2d[1] = y1-v1[1];
+		w2 = len_v2(v2d);
+		wtot = w1+w2;
+		uv[1] = w1/wtot;
+	} else {
+		/* lines are paralelle, lambda_cp_line_ex is 3d grrr */
+		/*printf("\tparalelle2\n");*/
+		pt3d[0] = pt[0];
+		pt3d[1] = pt[1];
+		pt3d[2] = l1[2] = l2[2] = 0.0f;
+		
+		
+		l1[0] = v0[0]; l1[1] = v0[1];
+		l2[0] = v3[0]; l2[1] = v3[1];
+		closest_to_line_v3(pt_on_line,pt3d, l1, l2);
+		v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
+		v2d[1] = pt[1]-pt_on_line[1];
+		w1 = len_v2(v2d);
+		
+		l1[0] = v1[0]; l1[1] = v1[1];
+		l2[0] = v2[0]; l2[1] = v2[1];
+		closest_to_line_v3(pt_on_line,pt3d, l1, l2);
+		v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */
+		v2d[1] = pt[1]-pt_on_line[1];
+		w2 = len_v2(v2d);
+		wtot = w1+w2;
+		uv[1] = w1/wtot;
+	}
+	/* may need to flip UV's here */
+}
+
+/* same as above but does tri's and quads, tri's are a bit of a hack */
+void isect_point_face_uv_v2(int isquad, float v0[2], float v1[2], float v2[2], float v3[2], float pt[2], float *uv)
+{
+	if (isquad) {
+		isect_point_quad_uv_v2(v0, v1, v2, v3, pt, uv);
+	}
+	else {
+		/* not for quads, use for our abuse of LineIntersectsTriangleUV */
+		float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3], lambda;
+			
+		p1_3d[0] = p2_3d[0] = uv[0];
+		p1_3d[1] = p2_3d[1] = uv[1];
+		p1_3d[2] = 1.0f;
+		p2_3d[2] = -1.0f;
+		v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
+		
+		/* generate a new fuv, (this is possibly a non optimal solution,
+		 * since we only need 2d calculation but use 3d func's)
+		 * 
+		 * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
+		 * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
+		 * This means the new values will be correct in relation to the derived meshes face. 
+		 */
+		copy_v2_v2(v0_3d, v0);
+		copy_v2_v2(v1_3d, v1);
+		copy_v2_v2(v2_3d, v2);
+		
+		/* Doing this in 3D is not nice */
+		isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
+	}
+}
+
+#if 0
+int isect_point_tri_v2(float v1[2], float v2[2], float v3[2], float pt[2])
+{
+	float inp1, inp2, inp3;
+	
+	inp1= (v2[0]-v1[0])*(v1[1]-pt[1]) + (v1[1]-v2[1])*(v1[0]-pt[0]);
+	inp2= (v3[0]-v2[0])*(v2[1]-pt[1]) + (v2[1]-v3[1])*(v2[0]-pt[0]);
+	inp3= (v1[0]-v3[0])*(v3[1]-pt[1]) + (v3[1]-v1[1])*(v3[0]-pt[0]);
+	
+	if(inp1<=0.0f && inp2<=0.0f && inp3<=0.0f) return 1;
+	if(inp1>=0.0f && inp2>=0.0f && inp3>=0.0f) return 1;
+	
+	return 0;
+}
+#endif
+
+#if 0
+int isect_point_tri_v2(float v0[2], float v1[2], float v2[2], float pt[2])
+{
+		/* not for quads, use for our abuse of LineIntersectsTriangleUV */
+		float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3];
+		/* not used */
+		float lambda, uv[3];
+			
+		p1_3d[0] = p2_3d[0] = uv[0]= pt[0];
+		p1_3d[1] = p2_3d[1] = uv[1]= uv[2]= pt[1];
+		p1_3d[2] = 1.0f;
+		p2_3d[2] = -1.0f;
+		v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0;
+		
+		/* generate a new fuv, (this is possibly a non optimal solution,
+		 * since we only need 2d calculation but use 3d func's)
+		 * 
+		 * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face
+		 * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV.
+		 * This means the new values will be correct in relation to the derived meshes face. 
+		 */
+		copy_v2_v2(v0_3d, v0);
+		copy_v2_v2(v1_3d, v1);
+		copy_v2_v2(v2_3d, v2);
+		
+		/* Doing this in 3D is not nice */
+		return isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv);
+}
+#endif
+
+/*
+
+	x1,y2
+	|  \
+	|   \     .(a,b)
+	|    \
+	x1,y1-- x2,y1
+
+*/
+int isect_point_tri_v2_int(int x1, int y1, int x2, int y2, int a, int b)
+{
+	float v1[2], v2[2], v3[2], p[2];
+	
+	v1[0]= (float)x1;
+	v1[1]= (float)y1;
+	
+	v2[0]= (float)x1;
+	v2[1]= (float)y2;
+	
+	v3[0]= (float)x2;
+	v3[1]= (float)y1;
+	
+	p[0]= (float)a;
+	p[1]= (float)b;
+	
+	return isect_point_tri_v2(v1, v2, v3, p);
+}
+
+static int point_in_slice(float p[3], float v1[3], float l1[3], float l2[3])
+{
+/* 
+what is a slice ?
+some maths:
+a line including l1,l2 and a point not on the line 
+define a subset of R3 delimeted by planes parallel to the line and orthogonal 
+to the (point --> line) distance vector,one plane on the line one on the point, 
+the room inside usually is rather small compared to R3 though still infinte
+useful for restricting (speeding up) searches 
+e.g. all points of triangular prism are within the intersection of 3 'slices'
+onother trivial case : cube 
+but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too
+*/
+	float h,rp[3],cp[3],q[3];
+
+	closest_to_line_v3(cp,v1,l1,l2);
+	sub_v3_v3v3(q,cp,v1);
+
+	sub_v3_v3v3(rp,p,v1);
+	h=dot_v3v3(q,rp)/dot_v3v3(q,q);
+	if (h < 0.0f || h > 1.0f) return 0;
+	return 1;
+}
+
+#if 0
+/*adult sister defining the slice planes by the origin and the normal  
+NOTE |normal| may not be 1 but defining the thickness of the slice*/
+static int point_in_slice_as(float p[3],float origin[3],float normal[3])
+{
+	float h,rp[3];
+	sub_v3_v3v3(rp,p,origin);
+	h=dot_v3v3(normal,rp)/dot_v3v3(normal,normal);
+	if (h < 0.0f || h > 1.0f) return 0;
+	return 1;
+}
+
+/*mama (knowing the squared lenght of the normal)*/
+static int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns)
+{
+	float h,rp[3];
+	sub_v3_v3v3(rp,p,origin);
+	h=dot_v3v3(normal,rp)/lns;
+	if (h < 0.0f || h > 1.0f) return 0;
+	return 1;
+}
+#endif
+
+int isect_point_tri_prism_v3(float p[3], float v1[3], float v2[3], float v3[3])
+{
+	if(!point_in_slice(p,v1,v2,v3)) return 0;
+	if(!point_in_slice(p,v2,v3,v1)) return 0;
+	if(!point_in_slice(p,v3,v1,v2)) return 0;
+	return 1;
+}
+
+/****************************** Interpolation ********************************/
+
+static float tri_signed_area(float *v1, float *v2, float *v3, int i, int j)
+{
+	return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i]));
+}
+
+static int barycentric_weights(float *v1, float *v2, float *v3, float *co, float *n, float *w)
+{
+	float xn, yn, zn, a1, a2, a3, asum;
+	short i, j;
+
+	/* find best projection of face XY, XZ or YZ: barycentric weights of
+	   the 2d projected coords are the same and faster to compute */
+	xn= (float)fabs(n[0]);
+	yn= (float)fabs(n[1]);
+	zn= (float)fabs(n[2]);
+	if(zn>=xn && zn>=yn) {i= 0; j= 1;}
+	else if(yn>=xn && yn>=zn) {i= 0; j= 2;}
+	else {i= 1; j= 2;} 
+
+	a1= tri_signed_area(v2, v3, co, i, j);
+	a2= tri_signed_area(v3, v1, co, i, j);
+	a3= tri_signed_area(v1, v2, co, i, j);
+
+	asum= a1 + a2 + a3;
+
+	if (fabs(asum) < FLT_EPSILON) {
+		/* zero area triangle */
+		w[0]= w[1]= w[2]= 1.0f/3.0f;
+		return 1;
+	}
+
+	asum= 1.0f/asum;
+	w[0]= a1*asum;
+	w[1]= a2*asum;
+	w[2]= a3*asum;
+
+	return 0;
+}
+
+void interp_weights_face_v3(float *w,float *v1, float *v2, float *v3, float *v4, float *co)
+{
+	float w2[3];
+
+	w[0]= w[1]= w[2]= w[3]= 0.0f;
+
+	/* first check for exact match */
+	if(equals_v3v3(co, v1))
+		w[0]= 1.0f;
+	else if(equals_v3v3(co, v2))
+		w[1]= 1.0f;
+	else if(equals_v3v3(co, v3))
+		w[2]= 1.0f;
+	else if(v4 && equals_v3v3(co, v4))
+		w[3]= 1.0f;
+	else {
+		/* otherwise compute barycentric interpolation weights */
+		float n1[3], n2[3], n[3];
+		int degenerate;
+
+		sub_v3_v3v3(n1, v1, v3);
+		if (v4) {
+			sub_v3_v3v3(n2, v2, v4);
+		}
+		else {
+			sub_v3_v3v3(n2, v2, v3);
+		}
+		cross_v3_v3v3(n, n1, n2);
+
+		/* OpenGL seems to split this way, so we do too */
+		if (v4) {
+			degenerate= barycentric_weights(v1, v2, v4, co, n, w);
+			SWAP(float, w[2], w[3]);
+
+			if(degenerate || (w[0] < 0.0f)) {
+				/* if w[1] is negative, co is on the other side of the v1-v3 edge,
+				   so we interpolate using the other triangle */
+				degenerate= barycentric_weights(v2, v3, v4, co, n, w2);
+
+				if(!degenerate) {
+					w[0]= 0.0f;
+					w[1]= w2[0];
+					w[2]= w2[1];
+					w[3]= w2[2];
+				}
+			}
+		}
+		else
+			barycentric_weights(v1, v2, v3, co, n, w);
+	}
+}
+
+/* Mean value weights - smooth interpolation weights for polygons with
+ * more than 3 vertices */
+static float mean_value_half_tan(float *v1, float *v2, float *v3)
+{
+	float d2[3], d3[3], cross[3], area, dot, len;
+
+	sub_v3_v3v3(d2, v2, v1);
+	sub_v3_v3v3(d3, v3, v1);
+	cross_v3_v3v3(cross, d2, d3);
+
+	area= len_v3(cross);
+	dot= dot_v3v3(d2, d3);
+	len= len_v3(d2)*len_v3(d3);
+
+	if(area == 0.0f)
+		return 0.0f;
+	else
+		return (len - dot)/area;
+}
+
+void interp_weights_poly_v3(float *w,float v[][3], int n, float *co)
+{
+	float totweight, t1, t2, len, *vmid, *vprev, *vnext;
+	int i;
+
+	totweight= 0.0f;
+
+	for(i=0; i<n; i++) {
+		vmid= v[i];
+		vprev= (i == 0)? v[n-1]: v[i-1];
+		vnext= (i == n-1)? v[0]: v[i+1];
+
+		t1= mean_value_half_tan(co, vprev, vmid);
+		t2= mean_value_half_tan(co, vmid, vnext);
+
+		len= len_v3v3(co, vmid);
+		w[i]= (t1+t2)/len;
+		totweight += w[i];
+	}
+
+	if(totweight != 0.0f)
+		for(i=0; i<n; i++)
+			w[i] /= totweight;
+}
+
+/* (x1,v1)(t1=0)------(x2,v2)(t2=1), 0<t<1 --> (x,v)(t) */
+void interp_cubic_v3(float *x, float *v,float *x1, float *v1, float *x2, float *v2, float t)
+{
+	float a[3],b[3];
+	float t2= t*t;
+	float t3= t2*t;
+
+	/* cubic interpolation */
+	a[0]= v1[0] + v2[0] + 2*(x1[0] - x2[0]);
+	a[1]= v1[1] + v2[1] + 2*(x1[1] - x2[1]);
+	a[2]= v1[2] + v2[2] + 2*(x1[2] - x2[2]);
+
+	b[0]= -2*v1[0] - v2[0] - 3*(x1[0] - x2[0]);
+	b[1]= -2*v1[1] - v2[1] - 3*(x1[1] - x2[1]);
+	b[2]= -2*v1[2] - v2[2] - 3*(x1[2] - x2[2]);
+
+	x[0]= a[0]*t3 + b[0]*t2 + v1[0]*t + x1[0];
+	x[1]= a[1]*t3 + b[1]*t2 + v1[1]*t + x1[1];
+	x[2]= a[2]*t3 + b[2]*t2 + v1[2]*t + x1[2];
+
+	v[0]= 3*a[0]*t2 + 2*b[0]*t + v1[0];
+	v[1]= 3*a[1]*t2 + 2*b[1]*t + v1[1];
+	v[2]= 3*a[2]*t2 + 2*b[2]*t + v1[2];
+}
+
+/***************************** View & Projection *****************************/
+
+void orthographic_m4(float matrix[][4],float left, float right, float bottom, float top, float nearClip, float farClip)
+{
+    float Xdelta, Ydelta, Zdelta;
+ 
+    Xdelta = right - left;
+    Ydelta = top - bottom;
+    Zdelta = farClip - nearClip;
+    if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
+		return;
+    }
+    unit_m4(matrix);
+    matrix[0][0] = 2.0f/Xdelta;
+    matrix[3][0] = -(right + left)/Xdelta;
+    matrix[1][1] = 2.0f/Ydelta;
+    matrix[3][1] = -(top + bottom)/Ydelta;
+    matrix[2][2] = -2.0f/Zdelta;		/* note: negate Z	*/
+    matrix[3][2] = -(farClip + nearClip)/Zdelta;
+}
+
+void perspective_m4(float mat[][4],float left, float right, float bottom, float top, float nearClip, float farClip)
+{
+	float Xdelta, Ydelta, Zdelta;
+
+	Xdelta = right - left;
+	Ydelta = top - bottom;
+	Zdelta = farClip - nearClip;
+
+	if (Xdelta == 0.0 || Ydelta == 0.0 || Zdelta == 0.0) {
+		return;
+	}
+	mat[0][0] = nearClip * 2.0f/Xdelta;
+	mat[1][1] = nearClip * 2.0f/Ydelta;
+	mat[2][0] = (right + left)/Xdelta;		/* note: negate Z	*/
+	mat[2][1] = (top + bottom)/Ydelta;
+	mat[2][2] = -(farClip + nearClip)/Zdelta;
+	mat[2][3] = -1.0f;
+	mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta;
+	mat[0][1] = mat[0][2] = mat[0][3] =
+	    mat[1][0] = mat[1][2] = mat[1][3] =
+	    mat[3][0] = mat[3][1] = mat[3][3] = 0.0;
+
+}
+
+static void i_multmatrix(float icand[][4], float Vm[][4])
+{
+    int row, col;
+    float temp[4][4];
+
+    for(row=0 ; row<4 ; row++) 
+        for(col=0 ; col<4 ; col++)
+            temp[row][col] = icand[row][0] * Vm[0][col]
+                           + icand[row][1] * Vm[1][col]
+                           + icand[row][2] * Vm[2][col]
+                           + icand[row][3] * Vm[3][col];
+	copy_m4_m4(Vm, temp);
+}
+
+
+void polarview_m4(float Vm[][4],float dist, float azimuth, float incidence, float twist)
+{
+
+	unit_m4(Vm);
+
+    translate_m4(Vm,0.0, 0.0, -dist);
+    rotate_m4(Vm,'z',-twist);	
+    rotate_m4(Vm,'x',-incidence);
+    rotate_m4(Vm,'z',-azimuth);
+}
+
+void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, float pz, float twist)
+{
+	float sine, cosine, hyp, hyp1, dx, dy, dz;
+	float mat1[4][4];
+	
+	unit_m4(mat);
+	unit_m4(mat1);
+
+	rotate_m4(mat,'z',-twist);
+
+	dx = px - vx;
+	dy = py - vy;
+	dz = pz - vz;
+	hyp = dx * dx + dz * dz;	/* hyp squared	*/
+	hyp1 = (float)sqrt(dy*dy + hyp);
+	hyp = (float)sqrt(hyp);		/* the real hyp	*/
+	
+	if (hyp1 != 0.0) {		/* rotate X	*/
+		sine = -dy / hyp1;
+		cosine = hyp /hyp1;
+	} else {
+		sine = 0;
+		cosine = 1.0f;
+	}
+	mat1[1][1] = cosine;
+	mat1[1][2] = sine;
+	mat1[2][1] = -sine;
+	mat1[2][2] = cosine;
+	
+	i_multmatrix(mat1, mat);
+
+    mat1[1][1] = mat1[2][2] = 1.0f;	/* be careful here to reinit	*/
+    mat1[1][2] = mat1[2][1] = 0.0;	/* those modified by the last	*/
+	
+	/* paragraph	*/
+	if (hyp != 0.0f) {			/* rotate Y	*/
+		sine = dx / hyp;
+		cosine = -dz / hyp;
+	} else {
+		sine = 0;
+		cosine = 1.0f;
+	}
+	mat1[0][0] = cosine;
+	mat1[0][2] = -sine;
+	mat1[2][0] = sine;
+	mat1[2][2] = cosine;
+	
+	i_multmatrix(mat1, mat);
+	translate_m4(mat,-vx,-vy,-vz);	/* translate viewpoint to origin */
+}
+
+/********************************** Mapping **********************************/
+
+void map_to_tube(float *u, float *v,float x, float y, float z)
+{
+	float len;
+	
+	*v = (z + 1.0f) / 2.0f;
+	
+	len= (float)sqrt(x*x+y*y);
+	if(len > 0.0f)
+		*u = (float)((1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0);
+	else
+		*v = *u = 0.0f; /* to avoid un-initialized variables */
+}
+
+void map_to_sphere(float *u, float *v,float x, float y, float z)
+{
+	float len;
+	
+	len= (float)sqrt(x*x+y*y+z*z);
+	if(len > 0.0f) {
+		if(x==0.0f && y==0.0f) *u= 0.0f;	/* othwise domain error */
+		else *u = (float)((1.0 - (float)atan2(x,y) / M_PI) / 2.0);
+		
+		z/=len;
+		*v = 1.0f - (float)saacos(z)/(float)M_PI;
+	} else {
+		*v = *u = 0.0f; /* to avoid un-initialized variables */
+	}
+}
+
+/********************************************************/
+
+/* Tangents */
+
+/* For normal map tangents we need to detect uv boundaries, and only average
+ * tangents in case the uvs are connected. Alternative would be to store 1 
+ * tangent per face rather than 4 per face vertex, but that's not compatible
+ * with games */
+
+
+/* from BKE_mesh.h */
+#define STD_UV_CONNECT_LIMIT	0.0001f
+
+#if 0
+void sum_or_add_vertex_tangent(void *arena, VertexTangent **vtang, float *tang, float *uv)
+{
+	VertexTangent *vt;
+
+	/* find a tangent with connected uvs */
+	for(vt= *vtang; vt; vt=vt->next) {
+		if(fabs(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabs(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT) {
+			add_v3_v3v3(vt->tang, vt->tang, tang);
+			return;
+		}
+	}
+
+	/* if not found, append a new one */
+	vt= BLI_memarena_alloc((MemArena *)arena, sizeof(VertexTangent));
+	copy_v3_v3(vt->tang, tang);
+	vt->uv[0]= uv[0];
+	vt->uv[1]= uv[1];
+
+	if(*vtang)
+		vt->next= *vtang;
+	*vtang= vt;
+}
+
+float *find_vertex_tangent(VertexTangent *vtang, float *uv)
+{
+	VertexTangent *vt;
+	static float nulltang[3] = {0.0f, 0.0f, 0.0f};
+
+	for(vt= vtang; vt; vt=vt->next)
+		if(fabs(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabs(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT)
+			return vt->tang;
+
+	return nulltang;	/* shouldn't happen, except for nan or so */
+}
+
+void tangent_from_uv(float *uv1, float *uv2, float *uv3, float *co1, float *co2, float *co3, float *n, float *tang)
+{
+	float tangv[3], ct[3], e1[3], e2[3], s1, t1, s2, t2, det;
+
+	s1= uv2[0] - uv1[0];
+	s2= uv3[0] - uv1[0];
+	t1= uv2[1] - uv1[1];
+	t2= uv3[1] - uv1[1];
+	det= 1.0f / (s1 * t2 - s2 * t1);
+	
+	/* normals in render are inversed... */
+	sub_v3_v3v3(e1, co1, co2);
+	sub_v3_v3v3(e2, co1, co3);
+	tang[0] = (t2*e1[0] - t1*e2[0])*det;
+	tang[1] = (t2*e1[1] - t1*e2[1])*det;
+	tang[2] = (t2*e1[2] - t1*e2[2])*det;
+	tangv[0] = (s1*e2[0] - s2*e1[0])*det;
+	tangv[1] = (s1*e2[1] - s2*e1[1])*det;
+	tangv[2] = (s1*e2[2] - s2*e1[2])*det;
+	cross_v3_v3v3(ct, tang, tangv);
+
+	/* check flip */
+	if ((ct[0]*n[0] + ct[1]*n[1] + ct[2]*n[2]) < 0.0f)
+		negate_v3(tang);
+}
+#endif
+
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c
new file mode 100644
index 00000000000..f25f24927b4
--- /dev/null
+++ b/source/blender/blenlib/intern/math_matrix.c
@@ -0,0 +1,1102 @@
+/*
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "BLI_math.h"
+
+/********************************* Init **************************************/
+
+void zero_m3(float *m)
+{
+	memset(m, 0, 3*3*sizeof(float));
+}
+
+void zero_m4(float *m)
+{
+	memset(m, 0, 4*4*sizeof(float));
+}
+
+void unit_m3(float m[][3])
+{
+	m[0][0]= m[1][1]= m[2][2]= 1.0;
+	m[0][1]= m[0][2]= 0.0;
+	m[1][0]= m[1][2]= 0.0;
+	m[2][0]= m[2][1]= 0.0;
+}
+
+void unit_m4(float m[][4])
+{
+	m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0;
+	m[0][1]= m[0][2]= m[0][3]= 0.0;
+	m[1][0]= m[1][2]= m[1][3]= 0.0;
+	m[2][0]= m[2][1]= m[2][3]= 0.0;
+	m[3][0]= m[3][1]= m[3][2]= 0.0;
+}
+
+void copy_m3_m3(float m1[][3], float m2[][3]) 
+{	
+	/* destination comes first: */
+	memcpy(&m1[0], &m2[0], 9*sizeof(float));
+}
+
+void copy_m4_m4(float m1[][4], float m2[][4]) 
+{
+	memcpy(m1, m2, 4*4*sizeof(float));
+}
+
+void copy_m3_m4(float m1[][3], float m2[][4])
+{
+	m1[0][0]= m2[0][0];
+	m1[0][1]= m2[0][1];
+	m1[0][2]= m2[0][2];
+
+	m1[1][0]= m2[1][0];
+	m1[1][1]= m2[1][1];
+	m1[1][2]= m2[1][2];
+
+	m1[2][0]= m2[2][0];
+	m1[2][1]= m2[2][1];
+	m1[2][2]= m2[2][2];
+}
+
+void copy_m4_m3(float m1[][4], float m2[][3])	/* no clear */
+{
+	m1[0][0]= m2[0][0];
+	m1[0][1]= m2[0][1];
+	m1[0][2]= m2[0][2];
+
+	m1[1][0]= m2[1][0];
+	m1[1][1]= m2[1][1];
+	m1[1][2]= m2[1][2];
+
+	m1[2][0]= m2[2][0];
+	m1[2][1]= m2[2][1];
+	m1[2][2]= m2[2][2];
+
+	/*	Reevan's Bugfix */
+	m1[0][3]=0.0F;
+	m1[1][3]=0.0F;
+	m1[2][3]=0.0F;
+
+	m1[3][0]=0.0F;	
+	m1[3][1]=0.0F;	
+	m1[3][2]=0.0F;	
+	m1[3][3]=1.0F;
+
+}
+
+void swap_m4m4(float m1[][4], float m2[][4])
+{
+	float t;
+	int i, j;
+
+	for(i = 0; i < 4; i++) {
+		for (j = 0; j < 4; j++) {
+			t        = m1[i][j];
+			m1[i][j] = m2[i][j];
+			m2[i][j] = t;
+		}
+	}
+}
+
+/******************************** Arithmetic *********************************/
+
+void mul_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
+{
+  /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */
+
+	m1[0][0] = m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0] + m2[0][3]*m3[3][0];
+	m1[0][1] = m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1] + m2[0][3]*m3[3][1];
+	m1[0][2] = m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2] + m2[0][3]*m3[3][2];
+	m1[0][3] = m2[0][0]*m3[0][3] + m2[0][1]*m3[1][3] + m2[0][2]*m3[2][3] + m2[0][3]*m3[3][3];
+
+	m1[1][0] = m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0] + m2[1][3]*m3[3][0];
+	m1[1][1] = m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1] + m2[1][3]*m3[3][1];
+	m1[1][2] = m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2] + m2[1][3]*m3[3][2];
+	m1[1][3] = m2[1][0]*m3[0][3] + m2[1][1]*m3[1][3] + m2[1][2]*m3[2][3] + m2[1][3]*m3[3][3];
+
+	m1[2][0] = m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0] + m2[2][3]*m3[3][0];
+	m1[2][1] = m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1] + m2[2][3]*m3[3][1];
+	m1[2][2] = m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2] + m2[2][3]*m3[3][2];
+	m1[2][3] = m2[2][0]*m3[0][3] + m2[2][1]*m3[1][3] + m2[2][2]*m3[2][3] + m2[2][3]*m3[3][3];
+
+	m1[3][0] = m2[3][0]*m3[0][0] + m2[3][1]*m3[1][0] + m2[3][2]*m3[2][0] + m2[3][3]*m3[3][0];
+	m1[3][1] = m2[3][0]*m3[0][1] + m2[3][1]*m3[1][1] + m2[3][2]*m3[2][1] + m2[3][3]*m3[3][1];
+	m1[3][2] = m2[3][0]*m3[0][2] + m2[3][1]*m3[1][2] + m2[3][2]*m3[2][2] + m2[3][3]*m3[3][2];
+	m1[3][3] = m2[3][0]*m3[0][3] + m2[3][1]*m3[1][3] + m2[3][2]*m3[2][3] + m2[3][3]*m3[3][3];
+
+}
+
+void mul_m3_m3m3(float m1[][3], float m3[][3], float m2[][3])
+{
+   /*  m1[i][j] = m2[i][k]*m3[k][j], args are flipped!  */
+  	m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; 
+  	m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; 
+  	m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; 
+
+  	m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; 
+  	m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; 
+  	m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; 
+
+  	m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; 
+  	m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1]; 
+  	m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2]; 
+}
+
+void mul_m4_m4m3(float (*m1)[4], float (*m3)[4], float (*m2)[3])
+{
+	m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
+	m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
+	m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
+	m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
+	m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
+	m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
+	m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
+	m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
+	m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
+}
+/* m1 = m2 * m3, ignore the elements on the 4th row/column of m3*/
+void mul_m3_m3m4(float m1[][3], float m2[][3], float m3[][4])
+{
+    /* m1[i][j] = m2[i][k] * m3[k][j] */
+    m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0];
+    m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1];
+    m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2];
+
+    m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0];
+    m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1];
+    m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2];
+
+    m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0];
+    m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] +m2[2][2] * m3[2][1];
+    m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] +m2[2][2] * m3[2][2];
+}
+
+
+
+void mul_m4_m3m4(float (*m1)[4], float (*m3)[3], float (*m2)[4])
+{
+	m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0];
+	m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1];
+	m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2];
+	m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0];
+	m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1];
+	m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2];
+	m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0];
+	m1[2][1]= m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1];
+	m1[2][2]= m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2];
+}
+
+void mul_serie_m3(float answ[][3],
+				   float m1[][3], float m2[][3], float m3[][3],
+				   float m4[][3], float m5[][3], float m6[][3],
+				   float m7[][3], float m8[][3])
+{
+	float temp[3][3];
+	
+	if(m1==0 || m2==0) return;
+
+	
+	mul_m3_m3m3(answ, m2, m1);
+	if(m3) {
+		mul_m3_m3m3(temp, m3, answ);
+		if(m4) {
+			mul_m3_m3m3(answ, m4, temp);
+			if(m5) {
+				mul_m3_m3m3(temp, m5, answ);
+				if(m6) {
+					mul_m3_m3m3(answ, m6, temp);
+					if(m7) {
+						mul_m3_m3m3(temp, m7, answ);
+						if(m8) {
+							mul_m3_m3m3(answ, m8, temp);
+						}
+						else copy_m3_m3(answ, temp);
+					}
+				}
+				else copy_m3_m3(answ, temp);
+			}
+		}
+		else copy_m3_m3(answ, temp);
+	}
+}
+
+void mul_serie_m4(float answ[][4], float m1[][4],
+				float m2[][4], float m3[][4], float m4[][4],
+				float m5[][4], float m6[][4], float m7[][4],
+				float m8[][4])
+{
+	float temp[4][4];
+	
+	if(m1==0 || m2==0) return;
+	
+	mul_m4_m4m4(answ, m2, m1);
+	if(m3) {
+		mul_m4_m4m4(temp, m3, answ);
+		if(m4) {
+			mul_m4_m4m4(answ, m4, temp);
+			if(m5) {
+				mul_m4_m4m4(temp, m5, answ);
+				if(m6) {
+					mul_m4_m4m4(answ, m6, temp);
+					if(m7) {
+						mul_m4_m4m4(temp, m7, answ);
+						if(m8) {
+							mul_m4_m4m4(answ, m8, temp);
+						}
+						else copy_m4_m4(answ, temp);
+					}
+				}
+				else copy_m4_m4(answ, temp);
+			}
+		}
+		else copy_m4_m4(answ, temp);
+	}
+}
+
+void mul_m4_v3(float mat[][4], float *vec)
+{
+	float x,y;
+
+	x=vec[0]; 
+	y=vec[1];
+	vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
+	vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
+	vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
+}
+
+void mul_v3_m4v3(float *in, float mat[][4], float *vec)
+{
+	float x,y;
+
+	x=vec[0]; 
+	y=vec[1];
+	in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0];
+	in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1];
+	in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2];
+}
+
+void mul_no_transl_m4v3(float mat[][4], float *vec)
+{
+	float x,y;
+
+	x= vec[0]; 
+	y= vec[1];
+	vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
+	vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
+	vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
+}
+
+void mul_project_m4_v4(float mat[][4], float *vec)
+{
+	float w;
+
+	w = vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3];
+	mul_m4_v3(mat, vec);
+
+	vec[0] /= w;
+	vec[1] /= w;
+	vec[2] /= w;
+}
+
+void mul_m4_v4(float mat[][4], float *vec)
+{
+	float x,y,z;
+
+	x=vec[0]; 
+	y=vec[1]; 
+	z= vec[2];
+	vec[0]=x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*vec[3];
+	vec[1]=x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*vec[3];
+	vec[2]=x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*vec[3];
+	vec[3]=x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*vec[3];
+}
+
+void mul_m3_v3(float mat[][3], float *vec)
+{
+	float x,y;
+
+	x=vec[0]; 
+	y=vec[1];
+	vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
+	vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
+	vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
+}
+
+void mul_transposed_m3_v3(float mat[][3], float *vec)
+{
+	float x,y;
+
+	x=vec[0]; 
+	y=vec[1];
+	vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2];
+	vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2];
+	vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2];
+}
+
+void mul_m3_fl(float *m, float f)
+{
+	int i;
+
+	for(i=0;i<9;i++) m[i]*=f;
+}
+
+void mul_m4_fl(float *m, float f)
+{
+	int i;
+
+	for(i=0;i<16;i++) m[i]*=f;	/* count to 12: without vector component */
+}
+
+void mul_no_transl_m4_fl(float *m, float f)		/* only scale component */
+{
+	int i,j;
+
+	for(i=0; i<3; i++) {
+		for(j=0; j<3; j++) {
+			
+			m[4*i+j] *= f;
+		}
+	}
+}
+
+void mul_m3_v3_double(float mat[][3], double *vec)
+{
+	double x,y;
+
+	x=vec[0]; 
+	y=vec[1];
+	vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2];
+	vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2];
+	vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2];
+}
+
+void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3])
+{
+	int i, j;
+
+	for(i=0;i<3;i++)
+		for(j=0;j<3;j++)
+			m1[i][j]= m2[i][j] + m3[i][j];
+}
+
+void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4])
+{
+	int i, j;
+
+	for(i=0;i<4;i++)
+		for(j=0;j<4;j++)
+			m1[i][j]= m2[i][j] + m3[i][j];
+}
+
+void invert_m3_m3(float m1[][3], float m2[][3])
+{
+	short a,b;
+	float det;
+
+	/* calc adjoint */
+	adjoint_m3_m3(m1,m2);
+
+	/* then determinant old matrix! */
+	det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1])
+	    -m2[1][0]* (m2[0][1]*m2[2][2] - m2[0][2]*m2[2][1])
+	    +m2[2][0]* (m2[0][1]*m2[1][2] - m2[0][2]*m2[1][1]);
+
+	if(det==0) det=1;
+	det= 1/det;
+	for(a=0;a<3;a++) {
+		for(b=0;b<3;b++) {
+			m1[a][b]*=det;
+		}
+	}
+}
+
+/*
+ * invertmat - 
+ * 		computes the inverse of mat and puts it in inverse.  Returns 
+ *	TRUE on success (i.e. can always find a pivot) and FALSE on failure.
+ * 	Uses Gaussian Elimination with partial (maximal column) pivoting.
+ *
+ *					Mark Segal - 1992
+ */
+
+int invert_m4_m4(float inverse[][4], float mat[][4])
+{
+	int i, j, k;
+	double temp;
+	float tempmat[4][4];
+	float max;
+	int maxj;
+
+	/* Set inverse to identity */
+	for (i=0; i<4; i++)
+		for (j=0; j<4; j++)
+			inverse[i][j] = 0;
+	for (i=0; i<4; i++)
+		inverse[i][i] = 1;
+
+	/* Copy original matrix so we don't mess it up */
+	for(i = 0; i < 4; i++)
+		for(j = 0; j <4; j++)
+			tempmat[i][j] = mat[i][j];
+
+	for(i = 0; i < 4; i++) {
+		/* Look for row with max pivot */
+		max = fabs(tempmat[i][i]);
+		maxj = i;
+		for(j = i + 1; j < 4; j++) {
+			if(fabs(tempmat[j][i]) > max) {
+				max = fabs(tempmat[j][i]);
+				maxj = j;
+			}
+		}
+		/* Swap rows if necessary */
+		if (maxj != i) {
+			for(k = 0; k < 4; k++) {
+				SWAP(float, tempmat[i][k], tempmat[maxj][k]);
+				SWAP(float, inverse[i][k], inverse[maxj][k]);
+			}
+		}
+
+		temp = tempmat[i][i];
+		if (temp == 0)
+			return 0;  /* No non-zero pivot */
+		for(k = 0; k < 4; k++) {
+			tempmat[i][k] = (float)(tempmat[i][k]/temp);
+			inverse[i][k] = (float)(inverse[i][k]/temp);
+		}
+		for(j = 0; j < 4; j++) {
+			if(j != i) {
+				temp = tempmat[j][i];
+				for(k = 0; k < 4; k++) {
+					tempmat[j][k] -= (float)(tempmat[i][k]*temp);
+					inverse[j][k] -= (float)(inverse[i][k]*temp);
+				}
+			}
+		}
+	}
+	return 1;
+}
+
+/****************************** Linear Algebra *******************************/
+
+void transpose_m3(float mat[][3])
+{
+	float t;
+
+	t = mat[0][1] ; 
+	mat[0][1] = mat[1][0] ; 
+	mat[1][0] = t;
+	t = mat[0][2] ; 
+	mat[0][2] = mat[2][0] ; 
+	mat[2][0] = t;
+	t = mat[1][2] ; 
+	mat[1][2] = mat[2][1] ; 
+	mat[2][1] = t;
+}
+
+void transpose_m4(float mat[][4])
+{
+	float t;
+
+	t = mat[0][1] ; 
+	mat[0][1] = mat[1][0] ; 
+	mat[1][0] = t;
+	t = mat[0][2] ; 
+	mat[0][2] = mat[2][0] ; 
+	mat[2][0] = t;
+	t = mat[0][3] ; 
+	mat[0][3] = mat[3][0] ; 
+	mat[3][0] = t;
+
+	t = mat[1][2] ; 
+	mat[1][2] = mat[2][1] ; 
+	mat[2][1] = t;
+	t = mat[1][3] ; 
+	mat[1][3] = mat[3][1] ; 
+	mat[3][1] = t;
+
+	t = mat[2][3] ; 
+	mat[2][3] = mat[3][2] ; 
+	mat[3][2] = t;
+}
+
+void orthogonalize_m3(float mat[][3], int axis)
+{
+	float size[3];
+	size[0] = len_v3(mat[0]);
+	size[1] = len_v3(mat[1]);
+	size[2] = len_v3(mat[2]);
+	normalize_v3(mat[axis]);
+	switch(axis)
+	{
+		case 0:
+			if (dot_v3v3(mat[0], mat[1]) < 1) {
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+				normalize_v3(mat[2]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			} else if (dot_v3v3(mat[0], mat[2]) < 1) {
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+				normalize_v3(mat[1]);
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+			} else {
+				float vec[3] = {mat[0][1], mat[0][2], mat[0][0]};
+
+				cross_v3_v3v3(mat[2], mat[0], vec);
+				normalize_v3(mat[2]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			}
+		case 1:
+			if (dot_v3v3(mat[1], mat[0]) < 1) {
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+				normalize_v3(mat[2]);
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+			} else if (dot_v3v3(mat[0], mat[2]) < 1) {
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+			} else {
+				float vec[3] = {mat[1][1], mat[1][2], mat[1][0]};
+
+				cross_v3_v3v3(mat[0], mat[1], vec);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+			}
+		case 2:
+			if (dot_v3v3(mat[2], mat[0]) < 1) {
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+				normalize_v3(mat[1]);
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+			} else if (dot_v3v3(mat[2], mat[1]) < 1) {
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			} else {
+				float vec[3] = {mat[2][1], mat[2][2], mat[2][0]};
+
+				cross_v3_v3v3(mat[0], vec, mat[2]);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			}
+	}
+	mul_v3_fl(mat[0], size[0]);
+	mul_v3_fl(mat[1], size[1]);
+	mul_v3_fl(mat[2], size[2]);
+}
+
+void orthogonalize_m4(float mat[][4], int axis)
+{
+	float size[3];
+	size[0] = len_v3(mat[0]);
+	size[1] = len_v3(mat[1]);
+	size[2] = len_v3(mat[2]);
+	normalize_v3(mat[axis]);
+	switch(axis)
+	{
+		case 0:
+			if (dot_v3v3(mat[0], mat[1]) < 1) {
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+				normalize_v3(mat[2]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			} else if (dot_v3v3(mat[0], mat[2]) < 1) {
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+				normalize_v3(mat[1]);
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+			} else {
+				float vec[3] = {mat[0][1], mat[0][2], mat[0][0]};
+
+				cross_v3_v3v3(mat[2], mat[0], vec);
+				normalize_v3(mat[2]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			}
+		case 1:
+			normalize_v3(mat[0]);
+			if (dot_v3v3(mat[1], mat[0]) < 1) {
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+				normalize_v3(mat[2]);
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+			} else if (dot_v3v3(mat[0], mat[2]) < 1) {
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+			} else {
+				float vec[3] = {mat[1][1], mat[1][2], mat[1][0]};
+
+				cross_v3_v3v3(mat[0], mat[1], vec);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[2], mat[0], mat[1]);
+			}
+		case 2:
+			if (dot_v3v3(mat[2], mat[0]) < 1) {
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+				normalize_v3(mat[1]);
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+			} else if (dot_v3v3(mat[2], mat[1]) < 1) {
+				cross_v3_v3v3(mat[0], mat[1], mat[2]);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			} else {
+				float vec[3] = {mat[2][1], mat[2][2], mat[2][0]};
+
+				cross_v3_v3v3(mat[0], vec, mat[2]);
+				normalize_v3(mat[0]);
+				cross_v3_v3v3(mat[1], mat[2], mat[0]);
+			}
+	}
+	mul_v3_fl(mat[0], size[0]);
+	mul_v3_fl(mat[1], size[1]);
+	mul_v3_fl(mat[2], size[2]);
+}
+
+int is_orthogonal_m3(float mat[][3])
+{
+	if (fabs(dot_v3v3(mat[0], mat[1])) > 1.5 * FLT_EPSILON)
+		return 0;
+
+	if (fabs(dot_v3v3(mat[1], mat[2])) > 1.5 * FLT_EPSILON)
+		return 0;
+
+	if (fabs(dot_v3v3(mat[0], mat[2])) > 1.5 * FLT_EPSILON)
+		return 0;
+	
+	return 1;
+}
+
+int is_orthogonal_m4(float mat[][4])
+{
+	if (fabs(dot_v3v3(mat[0], mat[1])) > 1.5 * FLT_EPSILON)
+		return 0;
+
+	if (fabs(dot_v3v3(mat[1], mat[2])) > 1.5 * FLT_EPSILON)
+		return 0;
+
+	if (fabs(dot_v3v3(mat[0], mat[2])) > 1.5 * FLT_EPSILON)
+		return 0;
+	
+	return 1;
+}
+
+void normalize_m3(float mat[][3])
+{	
+	normalize_v3(mat[0]);
+	normalize_v3(mat[1]);
+	normalize_v3(mat[2]);
+}
+
+void normalize_m4(float mat[][4])
+{
+	float len;
+	
+	len= normalize_v3(mat[0]);
+	if(len!=0.0) mat[0][3]/= len;
+	len= normalize_v3(mat[1]);
+	if(len!=0.0) mat[1][3]/= len;
+	len= normalize_v3(mat[2]);
+	if(len!=0.0) mat[2][3]/= len;
+}
+
+void adjoint_m3_m3(float m1[][3], float m[][3])
+{
+	m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1];
+	m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1];
+	m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1];
+
+	m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0];
+	m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0];
+	m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0];
+
+	m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0];
+	m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0];
+	m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0];
+}
+
+void adjoint_m4_m4(float out[][4], float in[][4])	/* out = ADJ(in) */
+{
+	float a1, a2, a3, a4, b1, b2, b3, b4;
+	float c1, c2, c3, c4, d1, d2, d3, d4;
+
+	a1= in[0][0]; 
+	b1= in[0][1];
+	c1= in[0][2]; 
+	d1= in[0][3];
+
+	a2= in[1][0]; 
+	b2= in[1][1];
+	c2= in[1][2]; 
+	d2= in[1][3];
+
+	a3= in[2][0]; 
+	b3= in[2][1];
+	c3= in[2][2]; 
+	d3= in[2][3];
+
+	a4= in[3][0]; 
+	b4= in[3][1];
+	c4= in[3][2]; 
+	d4= in[3][3];
+
+
+	out[0][0]  =   determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
+	out[1][0]  = - determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
+	out[2][0]  =   determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
+	out[3][0]  = - determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
+
+	out[0][1]  = - determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
+	out[1][1]  =   determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
+	out[2][1]  = - determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
+	out[3][1]  =   determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
+
+	out[0][2]  =   determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
+	out[1][2]  = - determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
+	out[2][2]  =   determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
+	out[3][2]  = - determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
+
+	out[0][3]  = - determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
+	out[1][3]  =   determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
+	out[2][3]  = - determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
+	out[3][3]  =   determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
+}
+
+float determinant_m2(float a,float b,float c,float d)
+{
+
+	return a*d - b*c;
+}
+
+float determinant_m3(float a1, float a2, float a3,
+			 float b1, float b2, float b3,
+			 float c1, float c2, float c3)
+{
+	float ans;
+
+	ans = a1 * determinant_m2(b2, b3, c2, c3)
+	    - b1 * determinant_m2(a2, a3, c2, c3)
+	    + c1 * determinant_m2(a2, a3, b2, b3);
+
+	return ans;
+}
+
+float determinant_m4(float m[][4])
+{
+	float ans;
+	float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4;
+
+	a1= m[0][0]; 
+	b1= m[0][1];
+	c1= m[0][2]; 
+	d1= m[0][3];
+
+	a2= m[1][0]; 
+	b2= m[1][1];
+	c2= m[1][2]; 
+	d2= m[1][3];
+
+	a3= m[2][0]; 
+	b3= m[2][1];
+	c3= m[2][2]; 
+	d3= m[2][3];
+
+	a4= m[3][0]; 
+	b4= m[3][1];
+	c4= m[3][2]; 
+	d4= m[3][3];
+
+	ans = a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
+	    - b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
+	    + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
+	    - d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
+
+	return ans;
+}
+
+/****************************** Transformations ******************************/
+
+void size_to_mat3(float mat[][3], float *size)
+{
+	mat[0][0]= size[0];
+	mat[0][1]= 0.0f;
+	mat[0][2]= 0.0f;
+	mat[1][1]= size[1];
+	mat[1][0]= 0.0f;
+	mat[1][2]= 0.0f;
+	mat[2][2]= size[2];
+	mat[2][1]= 0.0f;
+	mat[2][0]= 0.0f;
+}
+
+void size_to_mat4(float mat[][4], float *size)
+{
+	float tmat[3][3];
+	
+	size_to_mat3(tmat,size);
+	unit_m4(mat);
+	copy_m4_m3(mat, tmat);
+}
+
+void mat3_to_size(float *size, float mat[][3])
+{
+	size[0]= len_v3(mat[0]);
+	size[1]= len_v3(mat[1]);
+	size[2]= len_v3(mat[2]);
+}
+
+void mat4_to_size(float *size, float mat[][4])
+{
+	size[0]= len_v3(mat[0]);
+	size[1]= len_v3(mat[1]);
+	size[2]= len_v3(mat[2]);
+}
+
+/* this gets the average scale of a matrix, only use when your scaling
+ * data that has no idea of scale axis, examples are bone-envelope-radius
+ * and curve radius */
+float mat3_to_scale(float mat[][3])
+{
+	/* unit length vector */
+	float unit_vec[3] = {0.577350269189626f, 0.577350269189626f, 0.577350269189626f};
+	mul_m3_v3(mat, unit_vec);
+	return len_v3(unit_vec);
+}
+
+float mat4_to_scale(float mat[][4])
+{
+	float tmat[3][3];
+	copy_m3_m4(tmat, mat);
+	return mat3_to_scale(tmat);
+}
+
+void scale_m3_fl(float m[][3], float scale)
+{
+	m[0][0]= m[1][1]= m[2][2]= scale;
+	m[0][1]= m[0][2]= 0.0;
+	m[1][0]= m[1][2]= 0.0;
+	m[2][0]= m[2][1]= 0.0;
+}
+
+void scale_m4_fl(float m[][4], float scale)
+{
+	m[0][0]= m[1][1]= m[2][2]= scale;
+	m[3][3]= 1.0;
+	m[0][1]= m[0][2]= m[0][3]= 0.0;
+	m[1][0]= m[1][2]= m[1][3]= 0.0;
+	m[2][0]= m[2][1]= m[2][3]= 0.0;
+	m[3][0]= m[3][1]= m[3][2]= 0.0;
+}
+
+void translate_m4(float mat[][4],float Tx, float Ty, float Tz)
+{
+    mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]);
+    mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]);
+    mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]);
+}
+
+void rotate_m4(float mat[][4], char axis,float angle)
+{
+	int col;
+    float temp[4];
+    float cosine, sine;
+
+    for(col=0; col<4 ; col++)	/* init temp to zero matrix */
+        temp[col] = 0;
+
+    angle = (float)(angle*(3.1415926535/180.0));
+    cosine = (float)cos(angle);
+    sine = (float)sin(angle);
+    switch(axis){
+    case 'x':    
+    case 'X':    
+        for(col=0 ; col<4 ; col++)
+            temp[col] = cosine*mat[1][col] + sine*mat[2][col];
+        for(col=0 ; col<4 ; col++) {
+	    mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col];
+            mat[1][col] = temp[col];
+	}
+        break;
+
+    case 'y':
+    case 'Y':
+        for(col=0 ; col<4 ; col++)
+            temp[col] = cosine*mat[0][col] - sine*mat[2][col];
+        for(col=0 ; col<4 ; col++) {
+            mat[2][col] = sine*mat[0][col] + cosine*mat[2][col];
+            mat[0][col] = temp[col];
+        }
+	break;
+
+    case 'z':
+    case 'Z':
+        for(col=0 ; col<4 ; col++)
+            temp[col] = cosine*mat[0][col] + sine*mat[1][col];
+        for(col=0 ; col<4 ; col++) {
+            mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col];
+            mat[0][col] = temp[col];
+        }
+	break;
+    }
+}
+
+void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], float srcweight)
+{
+	float squat[4], dquat[4], fquat[4];
+	float ssize[3], dsize[3], fsize[4];
+	float rmat[3][3], smat[3][3];
+	
+	mat3_to_quat(dquat,dst);
+	mat3_to_size(dsize,dst);
+
+	mat3_to_quat(squat,src);
+	mat3_to_size(ssize,src);
+	
+	/* do blending */
+	interp_qt_qtqt(fquat, dquat, squat, srcweight);
+	interp_v3_v3v3(fsize, dsize, ssize, srcweight);
+
+	/* compose new matrix */
+	quat_to_mat3(rmat,fquat);
+	size_to_mat3(smat,fsize);
+	mul_m3_m3m3(out, rmat, smat);
+}
+
+void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], float srcweight)
+{
+	float squat[4], dquat[4], fquat[4];
+	float ssize[3], dsize[3], fsize[4];
+	float sloc[3], dloc[3], floc[3];
+	
+	mat4_to_quat(dquat,dst);
+	mat4_to_size(dsize,dst);
+	copy_v3_v3(dloc, dst[3]);
+
+	mat4_to_quat(squat,src);
+	mat4_to_size(ssize,src);
+	copy_v3_v3(sloc, src[3]);
+	
+	/* do blending */
+	interp_v3_v3v3(floc, dloc, sloc, srcweight);
+	interp_qt_qtqt(fquat, dquat, squat, srcweight);
+	interp_v3_v3v3(fsize, dsize, ssize, srcweight);
+
+	/* compose new matrix */
+	loc_quat_size_to_mat4(out, floc, fquat, fsize);
+}
+
+/* make a 4x4 matrix out of 3 transform components */
+/* matrices are made in the order: scale * rot * loc */
+// TODO: need to have a version that allows for rotation order...
+void loc_eul_size_to_mat4(float mat[4][4], float loc[3], float eul[3], float size[3])
+{
+	float rmat[3][3], smat[3][3], tmat[3][3];
+	
+	/* initialise new matrix */
+	unit_m4(mat);
+	
+	/* make rotation + scaling part */
+	eul_to_mat3(rmat,eul);
+	size_to_mat3(smat,size);
+	mul_m3_m3m3(tmat, rmat, smat);
+	
+	/* copy rot/scale part to output matrix*/
+	copy_m4_m3(mat, tmat);
+	
+	/* copy location to matrix */
+	mat[3][0] = loc[0];
+	mat[3][1] = loc[1];
+	mat[3][2] = loc[2];
+}
+
+/* make a 4x4 matrix out of 3 transform components */
+/* matrices are made in the order: scale * rot * loc */
+void loc_eulO_size_to_mat4(float mat[4][4], float loc[3], float eul[3], float size[3], short rotOrder)
+{
+	float rmat[3][3], smat[3][3], tmat[3][3];
+	
+	/* initialise new matrix */
+	unit_m4(mat);
+	
+	/* make rotation + scaling part */
+	eulO_to_mat3(rmat,eul, rotOrder);
+	size_to_mat3(smat,size);
+	mul_m3_m3m3(tmat, rmat, smat);
+	
+	/* copy rot/scale part to output matrix*/
+	copy_m4_m3(mat, tmat);
+	
+	/* copy location to matrix */
+	mat[3][0] = loc[0];
+	mat[3][1] = loc[1];
+	mat[3][2] = loc[2];
+}
+
+
+/* make a 4x4 matrix out of 3 transform components */
+/* matrices are made in the order: scale * rot * loc */
+void loc_quat_size_to_mat4(float mat[4][4], float loc[3], float quat[4], float size[3])
+{
+	float rmat[3][3], smat[3][3], tmat[3][3];
+	
+	/* initialise new matrix */
+	unit_m4(mat);
+	
+	/* make rotation + scaling part */
+	quat_to_mat3(rmat,quat);
+	size_to_mat3(smat,size);
+	mul_m3_m3m3(tmat, rmat, smat);
+	
+	/* copy rot/scale part to output matrix*/
+	copy_m4_m3(mat, tmat);
+	
+	/* copy location to matrix */
+	mat[3][0] = loc[0];
+	mat[3][1] = loc[1];
+	mat[3][2] = loc[2];
+}
+
+/*********************************** Other ***********************************/
+
+void print_m3(char *str, float m[][3])
+{
+	printf("%s\n", str);
+	printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]);
+	printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]);
+	printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]);
+	printf("\n");
+}
+
+void print_m4(char *str, float m[][4])
+{
+	printf("%s\n", str);
+	printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]);
+	printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]);
+	printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]);
+	printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]);
+	printf("\n");
+}
+
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
new file mode 100644
index 00000000000..85436d3a639
--- /dev/null
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -0,0 +1,1507 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ 
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "BLI_math.h"
+
+/******************************** Quaternions ********************************/
+
+void unit_qt(float *q)
+{
+	q[0]= 1.0f;
+	q[1]= q[2]= q[3]= 0.0f;
+}
+
+void copy_qt_qt(float *q1, float *q2)
+{
+	q1[0]= q2[0];
+	q1[1]= q2[1];
+	q1[2]= q2[2];
+	q1[3]= q2[3];
+}
+
+int is_zero_qt(float *q)
+{
+	return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0);
+}
+
+void mul_qt_qtqt(float *q, float *q1, float *q2)
+{
+	float t0,t1,t2;
+
+	t0=   q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
+	t1=   q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
+	t2=   q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
+	q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
+	q[0]=t0; 
+	q[1]=t1; 
+	q[2]=t2;
+}
+
+/* Assumes a unit quaternion */
+void mul_qt_v3(float *q, float *v)
+{
+	float t0, t1, t2;
+
+	t0=  -q[1]*v[0]-q[2]*v[1]-q[3]*v[2];
+	t1=   q[0]*v[0]+q[2]*v[2]-q[3]*v[1];
+	t2=   q[0]*v[1]+q[3]*v[0]-q[1]*v[2];
+	v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0];
+	v[0]=t1; 
+	v[1]=t2;
+
+	t1=   t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2];
+	t2=   t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3];
+	v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1];
+	v[0]=t1; 
+	v[1]=t2;
+}
+
+void conjugate_qt(float *q)
+{
+	q[1] = -q[1];
+	q[2] = -q[2];
+	q[3] = -q[3];
+}
+
+float dot_qtqt(float *q1, float *q2)
+{
+	return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3];
+}
+
+void invert_qt(float *q)
+{
+	float f = dot_qtqt(q, q);
+
+	if (f == 0.0f)
+		return;
+
+	conjugate_qt(q);
+	mul_qt_fl(q, 1.0f/f);
+}
+
+/* simple mult */
+void mul_qt_fl(float *q, float f)
+{
+	q[0] *= f;
+	q[1] *= f;
+	q[2] *= f;
+	q[3] *= f;
+}
+
+void sub_qt_qtqt(float *q, float *q1, float *q2)
+{
+	q2[0]= -q2[0];
+	mul_qt_qtqt(q, q1, q2);
+	q2[0]= -q2[0];
+}
+
+/* angular mult factor */
+void mul_fac_qt_fl(float *q, float fac)
+{
+	float angle= fac*saacos(q[0]);	/* quat[0]= cos(0.5*angle), but now the 0.5 and 2.0 rule out */
+	
+	float co= (float)cos(angle);
+	float si= (float)sin(angle);
+	q[0]= co;
+	normalize_v3(q+1);
+	q[1]*= si;
+	q[2]*= si;
+	q[3]*= si;
+	
+}
+
+void quat_to_mat3(float m[][3], float *q)
+{
+	double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
+
+	q0= M_SQRT2 * q[0];
+	q1= M_SQRT2 * q[1];
+	q2= M_SQRT2 * q[2];
+	q3= M_SQRT2 * q[3];
+
+	qda= q0*q1;
+	qdb= q0*q2;
+	qdc= q0*q3;
+	qaa= q1*q1;
+	qab= q1*q2;
+	qac= q1*q3;
+	qbb= q2*q2;
+	qbc= q2*q3;
+	qcc= q3*q3;
+
+	m[0][0]= (float)(1.0-qbb-qcc);
+	m[0][1]= (float)(qdc+qab);
+	m[0][2]= (float)(-qdb+qac);
+
+	m[1][0]= (float)(-qdc+qab);
+	m[1][1]= (float)(1.0-qaa-qcc);
+	m[1][2]= (float)(qda+qbc);
+
+	m[2][0]= (float)(qdb+qac);
+	m[2][1]= (float)(-qda+qbc);
+	m[2][2]= (float)(1.0-qaa-qbb);
+}
+
+void quat_to_mat4(float m[][4], float *q)
+{
+	double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
+
+	q0= M_SQRT2 * q[0];
+	q1= M_SQRT2 * q[1];
+	q2= M_SQRT2 * q[2];
+	q3= M_SQRT2 * q[3];
+
+	qda= q0*q1;
+	qdb= q0*q2;
+	qdc= q0*q3;
+	qaa= q1*q1;
+	qab= q1*q2;
+	qac= q1*q3;
+	qbb= q2*q2;
+	qbc= q2*q3;
+	qcc= q3*q3;
+
+	m[0][0]= (float)(1.0-qbb-qcc);
+	m[0][1]= (float)(qdc+qab);
+	m[0][2]= (float)(-qdb+qac);
+	m[0][3]= 0.0f;
+
+	m[1][0]= (float)(-qdc+qab);
+	m[1][1]= (float)(1.0-qaa-qcc);
+	m[1][2]= (float)(qda+qbc);
+	m[1][3]= 0.0f;
+
+	m[2][0]= (float)(qdb+qac);
+	m[2][1]= (float)(-qda+qbc);
+	m[2][2]= (float)(1.0-qaa-qbb);
+	m[2][3]= 0.0f;
+	
+	m[3][0]= m[3][1]= m[3][2]= 0.0f;
+	m[3][3]= 1.0f;
+}
+
+void mat3_to_quat(float *q,float wmat[][3])
+{
+	double tr, s;
+	float mat[3][3];
+
+	/* work on a copy */
+	copy_m3_m3(mat, wmat);
+	normalize_m3(mat);			/* this is needed AND a NormalQuat in the end */
+	
+	tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
+	
+	if(tr>FLT_EPSILON) {
+		s= sqrt(tr);
+		q[0]= (float)s;
+		s= 1.0/(4.0*s);
+		q[1]= (float)((mat[1][2]-mat[2][1])*s);
+		q[2]= (float)((mat[2][0]-mat[0][2])*s);
+		q[3]= (float)((mat[0][1]-mat[1][0])*s);
+	}
+	else {
+		if(mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
+			s= 2.0*sqrtf(1.0 + mat[0][0] - mat[1][1] - mat[2][2]);
+			q[1]= (float)(0.25*s);
+
+			s= 1.0/s;
+			q[0]= (float)((mat[2][1] - mat[1][2])*s);
+			q[2]= (float)((mat[1][0] + mat[0][1])*s);
+			q[3]= (float)((mat[2][0] + mat[0][2])*s);
+		}
+		else if(mat[1][1] > mat[2][2]) {
+			s= 2.0*sqrtf(1.0 + mat[1][1] - mat[0][0] - mat[2][2]);
+			q[2]= (float)(0.25*s);
+
+			s= 1.0/s;
+			q[0]= (float)((mat[2][0] - mat[0][2])*s);
+			q[1]= (float)((mat[1][0] + mat[0][1])*s);
+			q[3]= (float)((mat[2][1] + mat[1][2])*s);
+		}
+		else {
+			s= 2.0*sqrtf(1.0 + mat[2][2] - mat[0][0] - mat[1][1]);
+			q[3]= (float)(0.25*s);
+
+			s= 1.0/s;
+			q[0]= (float)((mat[1][0] - mat[0][1])*s);
+			q[1]= (float)((mat[2][0] + mat[0][2])*s);
+			q[2]= (float)((mat[2][1] + mat[1][2])*s);
+		}
+	}
+	normalize_qt(q);
+}
+
+#if 0
+void Mat3ToQuat_is_ok(float wmat[][3], float *q)
+{
+	float mat[3][3], matr[3][3], matn[3][3], q1[4], q2[4], angle, si, co, nor[3];
+
+	/* work on a copy */
+	copy_m3_m3(mat, wmat);
+	normalize_m3(mat);
+	
+	/* rotate z-axis of matrix to z-axis */
+
+	nor[0] = mat[2][1];		/* cross product with (0,0,1) */
+	nor[1] =  -mat[2][0];
+	nor[2] = 0.0;
+	normalize_v3(nor);
+	
+	co= mat[2][2];
+	angle= 0.5f*saacos(co);
+	
+	co= (float)cos(angle);
+	si= (float)sin(angle);
+	q1[0]= co;
+	q1[1]= -nor[0]*si;		/* negative here, but why? */
+	q1[2]= -nor[1]*si;
+	q1[3]= -nor[2]*si;
+
+	/* rotate back x-axis from mat, using inverse q1 */
+	quat_to_mat3(matr,q1);
+	invert_m3_m3(matn, matr);
+	mul_m3_v3(matn, mat[0]);
+	
+	/* and align x-axes */
+	angle= (float)(0.5*atan2(mat[0][1], mat[0][0]));
+	
+	co= (float)cos(angle);
+	si= (float)sin(angle);
+	q2[0]= co;
+	q2[1]= 0.0f;
+	q2[2]= 0.0f;
+	q2[3]= si;
+	
+	mul_qt_qtqt(q, q1, q2);
+}
+#endif
+
+void mat4_to_quat(float *q, float m[][4])
+{
+	float mat[3][3];
+	
+	copy_m3_m4(mat, m);
+	mat3_to_quat(q,mat);
+	
+}
+
+void normalize_qt(float *q)
+{
+	float len;
+	
+	len= (float)sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
+	if(len!=0.0) {
+		q[0]/= len;
+		q[1]/= len;
+		q[2]/= len;
+		q[3]/= len;
+	} else {
+		q[1]= 1.0f;
+		q[0]= q[2]= q[3]= 0.0f;			
+	}
+}
+
+void rotation_between_vecs_to_quat(float *q, float v1[3], float v2[3])
+{
+	float axis[3];
+	float angle;
+	
+	cross_v3_v3v3(axis, v1, v2);
+	
+	angle = angle_normalized_v3v3(v1, v2);
+	
+	axis_angle_to_quat(q, axis, angle);
+}
+
+void vec_to_quat(float *q,float *vec, short axis, short upflag)
+{
+	float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1;
+	
+	/* first rotate to axis */
+	if(axis>2) {	
+		x2= vec[0] ; y2= vec[1] ; z2= vec[2];
+		axis-= 3;
+	}
+	else {
+		x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2];
+	}
+	
+	q[0]=1.0; 
+	q[1]=q[2]=q[3]= 0.0;
+
+	len1= (float)sqrt(x2*x2+y2*y2+z2*z2);
+	if(len1 == 0.0) return;
+
+	/* nasty! I need a good routine for this...
+	 * problem is a rotation of an Y axis to the negative Y-axis for example.
+	 */
+
+	if(axis==0) {	/* x-axis */
+		nor[0]= 0.0;
+		nor[1]= -z2;
+		nor[2]= y2;
+
+		if(fabs(y2)+fabs(z2)<0.0001)
+			nor[1]= 1.0;
+
+		co= x2;
+	}
+	else if(axis==1) {	/* y-axis */
+		nor[0]= z2;
+		nor[1]= 0.0;
+		nor[2]= -x2;
+		
+		if(fabs(x2)+fabs(z2)<0.0001)
+			nor[2]= 1.0;
+		
+		co= y2;
+	}
+	else {			/* z-axis */
+		nor[0]= -y2;
+		nor[1]= x2;
+		nor[2]= 0.0;
+
+		if(fabs(x2)+fabs(y2)<0.0001)
+			nor[0]= 1.0;
+
+		co= z2;
+	}
+	co/= len1;
+
+	normalize_v3(nor);
+	
+	angle= 0.5f*saacos(co);
+	si= (float)sin(angle);
+	q[0]= (float)cos(angle);
+	q[1]= nor[0]*si;
+	q[2]= nor[1]*si;
+	q[3]= nor[2]*si;
+	
+	if(axis!=upflag) {
+		quat_to_mat3(mat,q);
+
+		fp= mat[2];
+		if(axis==0) {
+			if(upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1]));
+			else angle= (float)(-0.5*atan2(fp[1], fp[2]));
+		}
+		else if(axis==1) {
+			if(upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0]));
+			else angle= (float)(0.5*atan2(fp[0], fp[2]));
+		}
+		else {
+			if(upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0]));
+			else angle= (float)(-0.5*atan2(-fp[0], -fp[1]));
+		}
+				
+		co= (float)cos(angle);
+		si= (float)(sin(angle)/len1);
+		q2[0]= co;
+		q2[1]= x2*si;
+		q2[2]= y2*si;
+		q2[3]= z2*si;
+			
+		mul_qt_qtqt(q,q2,q);
+	}
+}
+
+#if 0
+/* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */
+void QuatInterpolW(float *result, float *quat1, float *quat2, float t)
+{
+	float omega, cosom, sinom, sc1, sc2;
+
+	cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
+	
+	/* rotate around shortest angle */
+	if ((1.0f + cosom) > 0.0001f) {
+		
+		if ((1.0f - cosom) > 0.0001f) {
+			omega = (float)acos(cosom);
+			sinom = (float)sin(omega);
+			sc1 = (float)sin((1.0 - t) * omega) / sinom;
+			sc2 = (float)sin(t * omega) / sinom;
+		} 
+		else {
+			sc1 = 1.0f - t;
+			sc2 = t;
+		}
+		result[0] = sc1*quat1[0] + sc2*quat2[0];
+		result[1] = sc1*quat1[1] + sc2*quat2[1];
+		result[2] = sc1*quat1[2] + sc2*quat2[2];
+		result[3] = sc1*quat1[3] + sc2*quat2[3];
+	} 
+	else {
+		result[0] = quat2[3];
+		result[1] = -quat2[2];
+		result[2] = quat2[1];
+		result[3] = -quat2[0];
+		
+		sc1 = (float)sin((1.0 - t)*M_PI_2);
+		sc2 = (float)sin(t*M_PI_2);
+		
+		result[0] = sc1*quat1[0] + sc2*result[0];
+		result[1] = sc1*quat1[1] + sc2*result[1];
+		result[2] = sc1*quat1[2] + sc2*result[2];
+		result[3] = sc1*quat1[3] + sc2*result[3];
+	}
+}
+#endif
+
+void interp_qt_qtqt(float *result, float *quat1, float *quat2, float t)
+{
+	float quat[4], omega, cosom, sinom, sc1, sc2;
+
+	cosom = quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3] ;
+	
+	/* rotate around shortest angle */
+	if (cosom < 0.0f) {
+		cosom = -cosom;
+		quat[0]= -quat1[0];
+		quat[1]= -quat1[1];
+		quat[2]= -quat1[2];
+		quat[3]= -quat1[3];
+	} 
+	else {
+		quat[0]= quat1[0];
+		quat[1]= quat1[1];
+		quat[2]= quat1[2];
+		quat[3]= quat1[3];
+	}
+	
+	if ((1.0f - cosom) > 0.0001f) {
+		omega = (float)acos(cosom);
+		sinom = (float)sin(omega);
+		sc1 = (float)sin((1 - t) * omega) / sinom;
+		sc2 = (float)sin(t * omega) / sinom;
+	} else {
+		sc1= 1.0f - t;
+		sc2= t;
+	}
+	
+	result[0] = sc1 * quat[0] + sc2 * quat2[0];
+	result[1] = sc1 * quat[1] + sc2 * quat2[1];
+	result[2] = sc1 * quat[2] + sc2 * quat2[2];
+	result[3] = sc1 * quat[3] + sc2 * quat2[3];
+}
+
+void add_qt_qtqt(float *result, float *quat1, float *quat2, float t)
+{
+	result[0]= quat1[0] + t*quat2[0];
+	result[1]= quat1[1] + t*quat2[1];
+	result[2]= quat1[2] + t*quat2[2];
+	result[3]= quat1[3] + t*quat2[3];
+}
+
+void tri_to_quat(float *quat, float *v1,  float *v2,  float *v3)
+{
+	/* imaginary x-axis, y-axis triangle is being rotated */
+	float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3];
+	
+	/* move z-axis to face-normal */
+	normal_tri_v3(vec,v1, v2, v3);
+
+	n[0]= vec[1];
+	n[1]= -vec[0];
+	n[2]= 0.0f;
+	normalize_v3(n);
+	
+	if(n[0]==0.0f && n[1]==0.0f) n[0]= 1.0f;
+	
+	angle= -0.5f*(float)saacos(vec[2]);
+	co= (float)cos(angle);
+	si= (float)sin(angle);
+	q1[0]= co;
+	q1[1]= n[0]*si;
+	q1[2]= n[1]*si;
+	q1[3]= 0.0f;
+	
+	/* rotate back line v1-v2 */
+	quat_to_mat3(mat,q1);
+	invert_m3_m3(imat, mat);
+	sub_v3_v3v3(vec, v2, v1);
+	mul_m3_v3(imat, vec);
+
+	/* what angle has this line with x-axis? */
+	vec[2]= 0.0f;
+	normalize_v3(vec);
+
+	angle= (float)(0.5*atan2(vec[1], vec[0]));
+	co= (float)cos(angle);
+	si= (float)sin(angle);
+	q2[0]= co;
+	q2[1]= 0.0f;
+	q2[2]= 0.0f;
+	q2[3]= si;
+	
+	mul_qt_qtqt(quat, q1, q2);
+}
+
+void print_qt(char *str,  float q[4])
+{
+	printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]);
+}
+
+/******************************** Axis Angle *********************************/
+
+/* Axis angle to Quaternions */
+void axis_angle_to_quat(float q[4], float axis[3], float angle)
+{
+	float nor[3];
+	float si;
+	
+	copy_v3_v3(nor, axis);
+	normalize_v3(nor);
+	
+	angle /= 2;
+	si = (float)sin(angle);
+	q[0] = (float)cos(angle);
+	q[1] = nor[0] * si;
+	q[2] = nor[1] * si;
+	q[3] = nor[2] * si;	
+}
+
+/* Quaternions to Axis Angle */
+void quat_to_axis_angle(float axis[3], float *angle,float q[4])
+{
+	float ha, si;
+	
+	/* calculate angle/2, and sin(angle/2) */
+	ha= (float)acos(q[0]);
+	si= (float)sin(ha);
+	
+	/* from half-angle to angle */
+	*angle= ha * 2;
+	
+	/* prevent division by zero for axis conversion */
+	if (fabs(si) < 0.0005)
+		si= 1.0f;
+	
+	axis[0]= q[1] / si;
+	axis[1]= q[2] / si;
+	axis[2]= q[3] / si;
+}
+
+/* Axis Angle to Euler Rotation */
+void axis_angle_to_eulO(float eul[3], short order,float axis[3], float angle)
+{
+	float q[4];
+	
+	/* use quaternions as intermediate representation for now... */
+	axis_angle_to_quat(q, axis, angle);
+	quat_to_eulO(eul, order,q);
+}
+
+/* Euler Rotation to Axis Angle */
+void eulO_to_axis_angle(float axis[3], float *angle,float eul[3], short order)
+{
+	float q[4];
+	
+	/* use quaternions as intermediate representation for now... */
+	eulO_to_quat(q,eul, order);
+	quat_to_axis_angle(axis, angle,q);
+}
+
+/* axis angle to 3x3 matrix - safer version (normalisation of axis performed) */
+void axis_angle_to_mat3(float mat[3][3],float axis[3], float angle)
+{
+	float nor[3], nsi[3], co, si, ico;
+	
+	/* normalise the axis first (to remove unwanted scaling) */
+	copy_v3_v3(nor, axis);
+	normalize_v3(nor);
+	
+	/* now convert this to a 3x3 matrix */
+	co= (float)cos(angle);		
+	si= (float)sin(angle);
+	
+	ico= (1.0f - co);
+	nsi[0]= nor[0]*si;
+	nsi[1]= nor[1]*si;
+	nsi[2]= nor[2]*si;
+	
+	mat[0][0] = ((nor[0] * nor[0]) * ico) + co;
+	mat[0][1] = ((nor[0] * nor[1]) * ico) + nsi[2];
+	mat[0][2] = ((nor[0] * nor[2]) * ico) - nsi[1];
+	mat[1][0] = ((nor[0] * nor[1]) * ico) - nsi[2];
+	mat[1][1] = ((nor[1] * nor[1]) * ico) + co;
+	mat[1][2] = ((nor[1] * nor[2]) * ico) + nsi[0];
+	mat[2][0] = ((nor[0] * nor[2]) * ico) + nsi[1];
+	mat[2][1] = ((nor[1] * nor[2]) * ico) - nsi[0];
+	mat[2][2] = ((nor[2] * nor[2]) * ico) + co;
+}
+
+/* axis angle to 4x4 matrix - safer version (normalisation of axis performed) */
+void axis_angle_to_mat4(float mat[4][4],float axis[3], float angle)
+{
+	float tmat[3][3];
+	
+	axis_angle_to_mat3(tmat,axis, angle);
+	unit_m4(mat);
+	copy_m4_m3(mat, tmat);
+}
+
+/* 3x3 matrix to axis angle (see Mat4ToVecRot too) */
+void mat3_to_axis_angle(float axis[3], float *angle,float mat[3][3])
+{
+	float q[4];
+	
+	/* use quaternions as intermediate representation */
+	// TODO: it would be nicer to go straight there...
+	mat3_to_quat(q,mat);
+	quat_to_axis_angle(axis, angle,q);
+}
+
+/* 4x4 matrix to axis angle (see Mat4ToVecRot too) */
+void mat4_to_axis_angle(float axis[3], float *angle,float mat[4][4])
+{
+	float q[4];
+	
+	/* use quaternions as intermediate representation */
+	// TODO: it would be nicer to go straight there...
+	mat4_to_quat(q,mat);
+	quat_to_axis_angle(axis, angle,q);
+}
+
+/****************************** Vector/Rotation ******************************/
+/* TODO: the following calls should probably be depreceated sometime         */
+
+/* 3x3 matrix to axis angle */
+void mat3_to_vec_rot(float axis[3], float *angle,float mat[3][3])
+{
+	float q[4];
+	
+	/* use quaternions as intermediate representation */
+	// TODO: it would be nicer to go straight there...
+	mat3_to_quat(q,mat);
+	quat_to_axis_angle(axis, angle,q);
+}
+
+/* 4x4 matrix to axis angle */
+void mat4_to_vec_rot(float axis[3], float *angle,float mat[4][4])
+{
+	float q[4];
+	
+	/* use quaternions as intermediate representation */
+	// TODO: it would be nicer to go straight there...
+	mat4_to_quat(q,mat);
+	quat_to_axis_angle(axis, angle,q);
+}
+
+/* axis angle to 3x3 matrix */
+void vec_rot_to_mat3(float mat[][3],float *vec, float phi)
+{
+	/* rotation of phi radials around vec */
+	float vx, vx2, vy, vy2, vz, vz2, co, si;
+	
+	vx= vec[0];
+	vy= vec[1];
+	vz= vec[2];
+	vx2= vx*vx;
+	vy2= vy*vy;
+	vz2= vz*vz;
+	co= (float)cos(phi);
+	si= (float)sin(phi);
+	
+	mat[0][0]= vx2+co*(1.0f-vx2);
+	mat[0][1]= vx*vy*(1.0f-co)+vz*si;
+	mat[0][2]= vz*vx*(1.0f-co)-vy*si;
+	mat[1][0]= vx*vy*(1.0f-co)-vz*si;
+	mat[1][1]= vy2+co*(1.0f-vy2);
+	mat[1][2]= vy*vz*(1.0f-co)+vx*si;
+	mat[2][0]= vz*vx*(1.0f-co)+vy*si;
+	mat[2][1]= vy*vz*(1.0f-co)-vx*si;
+	mat[2][2]= vz2+co*(1.0f-vz2);
+}
+
+/* axis angle to 4x4 matrix */
+void vec_rot_to_mat4(float mat[][4],float *vec, float phi)
+{
+	float tmat[3][3];
+	
+	vec_rot_to_mat3(tmat,vec, phi);
+	unit_m4(mat);
+	copy_m4_m3(mat, tmat);
+}
+
+/* axis angle to quaternion */
+void vec_rot_to_quat(float *quat,float *vec, float phi)
+{
+	/* rotation of phi radials around vec */
+	float si;
+
+	quat[1]= vec[0];
+	quat[2]= vec[1];
+	quat[3]= vec[2];
+	
+	if(normalize_v3(quat+1) == 0.0f) {
+		unit_qt(quat);
+	}
+	else {
+		quat[0]= (float)cos(phi/2.0);
+		si= (float)sin(phi/2.0);
+		quat[1] *= si;
+		quat[2] *= si;
+		quat[3] *= si;
+	}
+}
+
+/******************************** XYZ Eulers *********************************/
+
+/* XYZ order */
+void eul_to_mat3(float mat[][3], float *eul)
+{
+	double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+	
+	ci = cos(eul[0]); 
+	cj = cos(eul[1]); 
+	ch = cos(eul[2]);
+	si = sin(eul[0]); 
+	sj = sin(eul[1]); 
+	sh = sin(eul[2]);
+	cc = ci*ch; 
+	cs = ci*sh; 
+	sc = si*ch; 
+	ss = si*sh;
+
+	mat[0][0] = (float)(cj*ch); 
+	mat[1][0] = (float)(sj*sc-cs); 
+	mat[2][0] = (float)(sj*cc+ss);
+	mat[0][1] = (float)(cj*sh); 
+	mat[1][1] = (float)(sj*ss+cc); 
+	mat[2][1] = (float)(sj*cs-sc);
+	mat[0][2] = (float)-sj;	 
+	mat[1][2] = (float)(cj*si);    
+	mat[2][2] = (float)(cj*ci);
+
+}
+
+/* XYZ order */
+void eul_to_mat4(float mat[][4], float *eul)
+{
+	double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+	
+	ci = cos(eul[0]); 
+	cj = cos(eul[1]); 
+	ch = cos(eul[2]);
+	si = sin(eul[0]); 
+	sj = sin(eul[1]); 
+	sh = sin(eul[2]);
+	cc = ci*ch; 
+	cs = ci*sh; 
+	sc = si*ch; 
+	ss = si*sh;
+
+	mat[0][0] = (float)(cj*ch); 
+	mat[1][0] = (float)(sj*sc-cs); 
+	mat[2][0] = (float)(sj*cc+ss);
+	mat[0][1] = (float)(cj*sh); 
+	mat[1][1] = (float)(sj*ss+cc); 
+	mat[2][1] = (float)(sj*cs-sc);
+	mat[0][2] = (float)-sj;	 
+	mat[1][2] = (float)(cj*si);    
+	mat[2][2] = (float)(cj*ci);
+
+
+	mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f;
+	mat[3][3]= 1.0f;
+}
+
+/* returns two euler calculation methods, so we can pick the best */
+/* XYZ order */
+static void mat3_to_eul2(float tmat[][3], float *eul1, float *eul2)
+{
+	float cy, quat[4], mat[3][3];
+	
+	mat3_to_quat(quat,tmat);
+	quat_to_mat3(mat,quat);
+	copy_m3_m3(mat, tmat);
+	normalize_m3(mat);
+	
+	cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
+	
+	if (cy > 16.0*FLT_EPSILON) {
+		
+		eul1[0] = (float)atan2(mat[1][2], mat[2][2]);
+		eul1[1] = (float)atan2(-mat[0][2], cy);
+		eul1[2] = (float)atan2(mat[0][1], mat[0][0]);
+		
+		eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]);
+		eul2[1] = (float)atan2(-mat[0][2], -cy);
+		eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]);
+		
+	} else {
+		eul1[0] = (float)atan2(-mat[2][1], mat[1][1]);
+		eul1[1] = (float)atan2(-mat[0][2], cy);
+		eul1[2] = 0.0f;
+		
+		copy_v3_v3(eul2, eul1);
+	}
+}
+
+/* XYZ order */
+void mat3_to_eul(float *eul,float tmat[][3])
+{
+	float eul1[3], eul2[3];
+	
+	mat3_to_eul2(tmat, eul1, eul2);
+		
+	/* return best, which is just the one with lowest values it in */
+	if(fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) {
+		copy_v3_v3(eul, eul2);
+	}
+	else {
+		copy_v3_v3(eul, eul1);
+	}
+}
+
+/* XYZ order */
+void mat4_to_eul(float *eul,float tmat[][4])
+{
+	float tempMat[3][3];
+
+	copy_m3_m4(tempMat, tmat);
+	normalize_m3(tempMat);
+	mat3_to_eul(eul,tempMat);
+}
+
+/* XYZ order */
+void quat_to_eul(float *eul,float *quat)
+{
+	float mat[3][3];
+	
+	quat_to_mat3(mat,quat);
+	mat3_to_eul(eul,mat);
+}
+
+/* XYZ order */
+void eul_to_quat(float *quat,float *eul)
+{
+    float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+ 
+    ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f;
+    ci = (float)cos(ti);  cj = (float)cos(tj);  ch = (float)cos(th);
+    si = (float)sin(ti);  sj = (float)sin(tj);  sh = (float)sin(th);
+    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
+	
+	quat[0] = cj*cc + sj*ss;
+	quat[1] = cj*sc - sj*cs;
+	quat[2] = cj*ss + sj*cc;
+	quat[3] = cj*cs - sj*sc;
+}
+
+/* XYZ order */
+void rotate_eul(float *beul, char axis, float ang)
+{
+	float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
+	
+	eul[0]= eul[1]= eul[2]= 0.0f;
+	if(axis=='x') eul[0]= ang;
+	else if(axis=='y') eul[1]= ang;
+	else eul[2]= ang;
+	
+	eul_to_mat3(mat1,eul);
+	eul_to_mat3(mat2,beul);
+	
+	mul_m3_m3m3(totmat, mat2, mat1);
+	
+	mat3_to_eul(beul,totmat);
+	
+}
+
+#if 0
+/* exported to transform.c */
+/* order independent! */
+void compatible_eul(float *eul, float *oldrot)
+{
+	float dx, dy, dz;
+	
+	/* correct differences of about 360 degrees first */
+	dx= eul[0] - oldrot[0];
+	dy= eul[1] - oldrot[1];
+	dz= eul[2] - oldrot[2];
+	
+	while(fabs(dx) > 5.1) {
+		if(dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI;
+		dx= eul[0] - oldrot[0];
+	}
+	while(fabs(dy) > 5.1) {
+		if(dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI;
+		dy= eul[1] - oldrot[1];
+	}
+	while(fabs(dz) > 5.1) {
+		if(dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI;
+		dz= eul[2] - oldrot[2];
+	}
+	
+	/* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */	
+	if(fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6) {
+		if(dx > 0.0) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI;
+	}
+	if(fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6) {
+		if(dy > 0.0) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI;
+	}
+	if(fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6) {
+		if(dz > 0.0) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI;
+	}
+	
+	/* the method below was there from ancient days... but why! probably because the code sucks :)
+		*/
+#if 0	
+	/* calc again */
+	dx= eul[0] - oldrot[0];
+	dy= eul[1] - oldrot[1];
+	dz= eul[2] - oldrot[2];
+	
+	/* special case, tested for x-z  */
+	
+	if((fabs(dx) > 3.1 && fabs(dz) > 1.5) || (fabs(dx) > 1.5 && fabs(dz) > 3.1)) {
+		if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
+		if(eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1];
+		if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
+		
+	}
+	else if((fabs(dx) > 3.1 && fabs(dy) > 1.5) || (fabs(dx) > 1.5 && fabs(dy) > 3.1)) {
+		if(dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI;
+		if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
+		if(eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2];
+	}
+	else if((fabs(dy) > 3.1 && fabs(dz) > 1.5) || (fabs(dy) > 1.5 && fabs(dz) > 3.1)) {
+		if(eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0];
+		if(dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI;
+		if(dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI;
+	}
+#endif	
+}
+#endif
+
+/* uses 2 methods to retrieve eulers, and picks the closest */
+/* XYZ order */
+void mat3_to_compatible_eul(float *eul, float *oldrot,float mat[][3])
+{
+	float eul1[3], eul2[3];
+	float d1, d2;
+	
+	mat3_to_eul2(mat, eul1, eul2);
+	
+	compatible_eul(eul1, oldrot);
+	compatible_eul(eul2, oldrot);
+	
+	d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (float)fabs(eul1[2]-oldrot[2]);
+	d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (float)fabs(eul2[2]-oldrot[2]);
+	
+	/* return best, which is just the one with lowest difference */
+	if(d1 > d2) {
+		copy_v3_v3(eul, eul2);
+	}
+	else {
+		copy_v3_v3(eul, eul1);
+	}
+	
+}
+
+/************************** Arbitrary Order Eulers ***************************/
+
+/* Euler Rotation Order Code:
+ * was adapted from  
+  		ANSI C code from the article
+		"Euler Angle Conversion"
+		by Ken Shoemake, shoemake@graphics.cis.upenn.edu
+		in "Graphics Gems IV", Academic Press, 1994
+ * for use in Blender
+ */
+
+/* Type for rotation order info - see wiki for derivation details */
+typedef struct RotOrderInfo {
+	short axis[3];
+	short parity;	/* parity of axis permutation (even=0, odd=1) - 'n' in original code */
+} RotOrderInfo;
+
+/* Array of info for Rotation Order calculations 
+ * WARNING: must be kept in same order as eEulerRotationOrders
+ */
+static RotOrderInfo rotOrders[]= {
+	/* i, j, k, n */
+	{{0, 1, 2}, 0}, // XYZ
+	{{0, 2, 1}, 1}, // XZY
+	{{1, 0, 2}, 1}, // YXZ
+	{{1, 2, 0}, 0}, // YZX
+	{{2, 0, 1}, 0}, // ZXY
+	{{2, 1, 0}, 1}  // ZYZ
+};
+
+/* Get relevant pointer to rotation order set from the array 
+ * NOTE: since we start at 1 for the values, but arrays index from 0, 
+ *		 there is -1 factor involved in this process...
+ */
+#define GET_ROTATIONORDER_INFO(order) (((order)>=1) ? &rotOrders[(order)-1] : &rotOrders[0])
+
+/* Construct quaternion from Euler angles (in radians). */
+void eulO_to_quat(float q[4],float e[3], short order)
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
+	short i=R->axis[0],  j=R->axis[1], 	k=R->axis[2];
+	double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+	double a[3];
+	
+	ti = e[i]/2; tj = e[j]/2; th = e[k]/2;
+	
+	if (R->parity) e[j] = -e[j];
+	
+	ci = cos(ti);  cj = cos(tj);  ch = cos(th);
+	si = sin(ti);  sj = sin(tj);  sh = sin(th);
+	
+	cc = ci*ch; cs = ci*sh; 
+	sc = si*ch; ss = si*sh;
+	
+	a[i] = cj*sc - sj*cs;
+	a[j] = cj*ss + sj*cc;
+	a[k] = cj*cs - sj*sc;
+	
+	q[0] = cj*cc + sj*ss;
+	q[1] = a[0];
+	q[2] = a[1];
+	q[3] = a[2];
+	
+	if (R->parity) q[j] = -q[j];
+}
+
+/* Convert quaternion to Euler angles (in radians). */
+void quat_to_eulO(float e[3], short order,float q[4])
+{
+	float M[3][3];
+	
+	quat_to_mat3(M,q);
+	mat3_to_eulO(e, order,M);
+}
+
+/* Construct 3x3 matrix from Euler angles (in radians). */
+void eulO_to_mat3(float M[3][3],float e[3], short order)
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
+	short i=R->axis[0],  j=R->axis[1], 	k=R->axis[2];
+	double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
+	
+	if (R->parity) {
+		ti = -e[i];	  tj = -e[j];	th = -e[k];
+	}
+	else {
+		ti = e[i];	  tj = e[j];	th = e[k];
+	}
+	
+	ci = cos(ti); cj = cos(tj); ch = cos(th);
+	si = sin(ti); sj = sin(tj); sh = sin(th);
+	
+	cc = ci*ch; cs = ci*sh; 
+	sc = si*ch; ss = si*sh;
+	
+	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
+	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
+	M[i][k] = -sj;	 M[j][k] = cj*si;	 M[k][k] = cj*ci;
+}
+
+/* Construct 4x4 matrix from Euler angles (in radians). */
+void eulO_to_mat4(float M[4][4],float e[3], short order)
+{
+	float m[3][3];
+	
+	/* for now, we'll just do this the slow way (i.e. copying matrices) */
+	normalize_m3(m);
+	eulO_to_mat3(m,e, order);
+	copy_m4_m3(M, m);
+}
+
+/* Convert 3x3 matrix to Euler angles (in radians). */
+void mat3_to_eulO(float e[3], short order,float M[3][3])
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
+	short i=R->axis[0],  j=R->axis[1], 	k=R->axis[2];
+	double cy = sqrt(M[i][i]*M[i][i] + M[i][j]*M[i][j]);
+	
+	if (cy > 16*FLT_EPSILON) {
+		e[i] = atan2(M[j][k], M[k][k]);
+		e[j] = atan2(-M[i][k], cy);
+		e[k] = atan2(M[i][j], M[i][i]);
+	} 
+	else {
+		e[i] = atan2(-M[k][j], M[j][j]);
+		e[j] = atan2(-M[i][k], cy);
+		e[k] = 0;
+	}
+	
+	if (R->parity) {
+		e[0] = -e[0]; 
+		e[1] = -e[1]; 
+		e[2] = -e[2];
+	}
+}
+
+/* Convert 4x4 matrix to Euler angles (in radians). */
+void mat4_to_eulO(float e[3], short order,float M[4][4])
+{
+	float m[3][3];
+	
+	/* for now, we'll just do this the slow way (i.e. copying matrices) */
+	copy_m3_m4(m, M);
+	normalize_m3(m);
+	mat3_to_eulO(e, order,m);
+}
+
+/* returns two euler calculation methods, so we can pick the best */
+static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order)
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); 
+	short i=R->axis[0],  j=R->axis[1], 	k=R->axis[2];
+	float m[3][3];
+	double cy;
+	
+	/* process the matrix first */
+	copy_m3_m3(m, M);
+	normalize_m3(m);
+	
+	cy= sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]);
+	
+	if (cy > 16*FLT_EPSILON) {
+		e1[i] = atan2(m[j][k], m[k][k]);
+		e1[j] = atan2(-m[i][k], cy);
+		e1[k] = atan2(m[i][j], m[i][i]);
+		
+		e2[i] = atan2(-m[j][k], -m[k][k]);
+		e2[j] = atan2(-m[i][k], -cy);
+		e2[k] = atan2(-m[i][j], -m[i][i]);
+	} 
+	else {
+		e1[i] = atan2(-m[k][j], m[j][j]);
+		e1[j] = atan2(-m[i][k], cy);
+		e1[k] = 0;
+		
+		copy_v3_v3(e2, e1);
+	}
+	
+	if (R->parity) {
+		e1[0] = -e1[0]; 
+		e1[1] = -e1[1]; 
+		e1[2] = -e1[2];
+		
+		e2[0] = -e2[0]; 
+		e2[1] = -e2[1]; 
+		e2[2] = -e2[2];
+	}
+}
+
+/* uses 2 methods to retrieve eulers, and picks the closest */
+void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float mat[3][3])
+{
+	float eul1[3], eul2[3];
+	float d1, d2;
+	
+	mat3_to_eulo2(mat, eul1, eul2, order);
+	
+	compatible_eul(eul1, oldrot);
+	compatible_eul(eul2, oldrot);
+	
+	d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (float)fabs(eul1[2]-oldrot[2]);
+	d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (float)fabs(eul2[2]-oldrot[2]);
+	
+	/* return best, which is just the one with lowest difference */
+	if (d1 > d2)
+		copy_v3_v3(eul, eul2);
+	else
+		copy_v3_v3(eul, eul1);
+}
+
+/* rotate the given euler by the given angle on the specified axis */
+// NOTE: is this safe to do with different axis orders?
+void rotate_eulO(float beul[3], short order, char axis, float ang)
+{
+	float eul[3], mat1[3][3], mat2[3][3], totmat[3][3];
+	
+	eul[0]= eul[1]= eul[2]= 0.0f;
+	if (axis=='x') 
+		eul[0]= ang;
+	else if (axis=='y') 
+		eul[1]= ang;
+	else 
+		eul[2]= ang;
+	
+	eulO_to_mat3(mat1,eul, order);
+	eulO_to_mat3(mat2,beul, order);
+	
+	mul_m3_m3m3(totmat, mat2, mat1);
+	
+	mat3_to_eulO(beul, order,totmat);
+}
+
+/* the matrix is written to as 3 axis vectors */
+void eulO_to_gimbal_axis(float gmat[][3], float *eul, short order)
+{
+	RotOrderInfo *R= GET_ROTATIONORDER_INFO(order);
+
+	float mat[3][3];
+	float teul[3];
+
+	/* first axis is local */
+	eulO_to_mat3(mat,eul, order);
+	copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]);
+	
+	/* second axis is local minus first rotation */
+	copy_v3_v3(teul, eul);
+	teul[R->axis[0]] = 0;
+	eulO_to_mat3(mat,teul, order);
+	copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]);
+	
+	
+	/* Last axis is global */
+	gmat[R->axis[2]][0] = 0;
+	gmat[R->axis[2]][1] = 0;
+	gmat[R->axis[2]][2] = 0;
+	gmat[R->axis[2]][R->axis[2]] = 1;
+}
+
+/******************************* Dual Quaternions ****************************/
+
+/*
+   Conversion routines between (regular quaternion, translation) and
+   dual quaternion.
+
+   Version 1.0.0, February 7th, 2007
+
+   Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights 
+   Reserved
+
+   This software is provided 'as-is', without any express or implied
+   warranty.  In no event will the author(s) be held liable for any damages
+   arising from the use of this software.
+
+   Permission is granted to anyone to use this software for any purpose,
+   including commercial applications, and to alter it and redistribute it
+   freely, subject to the following restrictions:
+
+   1. The origin of this software must not be misrepresented; you must not
+      claim that you wrote the original software. If you use this software
+      in a product, an acknowledgment in the product documentation would be
+      appreciated but is not required.
+   2. Altered source versions must be plainly marked as such, and must not be
+      misrepresented as being the original software.
+   3. This notice may not be removed or altered from any source distribution.
+
+   Author: Ladislav Kavan, kavanl@cs.tcd.ie
+
+   Changes for Blender:
+   - renaming, style changes and optimizations
+   - added support for scaling
+*/
+
+void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4])
+{
+	float *t, *q, dscale[3], scale[3], basequat[4];
+	float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4];
+	float R[4][4], S[4][4];
+
+	/* split scaling and rotation, there is probably a faster way to do
+	   this, it's done like this now to correctly get negative scaling */
+	mul_m4_m4m4(baseRS, basemat, mat);
+	mat4_to_size(scale,baseRS);
+
+	copy_v3_v3(dscale, scale);
+	dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f;
+
+	if((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4) {
+		/* extract R and S  */
+		mat4_to_quat(basequat,baseRS);
+		quat_to_mat4(baseR,basequat);
+		copy_v3_v3(baseR[3], baseRS[3]);
+
+		invert_m4_m4(baseinv, basemat);
+		mul_m4_m4m4(R, baseinv, baseR);
+
+		invert_m4_m4(baseRinv, baseR);
+		mul_m4_m4m4(S, baseRS, baseRinv);
+
+		/* set scaling part */
+		mul_serie_m4(dq->scale, basemat, S, baseinv, 0, 0, 0, 0, 0);
+		dq->scale_weight= 1.0f;
+	}
+	else {
+		/* matrix does not contain scaling */
+		copy_m4_m4(R, mat);
+		dq->scale_weight= 0.0f;
+	}
+
+	/* non-dual part */
+	mat4_to_quat(dq->quat,R);
+
+	/* dual part */
+	t= R[3];
+	q= dq->quat;
+	dq->trans[0]= -0.5f*(t[0]*q[1] + t[1]*q[2] + t[2]*q[3]);
+	dq->trans[1]=  0.5f*(t[0]*q[0] + t[1]*q[3] - t[2]*q[2]);
+	dq->trans[2]=  0.5f*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]);
+	dq->trans[3]=  0.5f*(t[0]*q[2] - t[1]*q[1] + t[2]*q[0]);
+}
+
+void dquat_to_mat4(float mat[][4],DualQuat *dq)
+{
+	float len, *t, q0[4];
+	
+	/* regular quaternion */
+	copy_qt_qt(q0, dq->quat);
+
+	/* normalize */
+	len= (float)sqrt(dot_qtqt(q0, q0)); 
+	if(len != 0.0f)
+		mul_qt_fl(q0, 1.0f/len);
+	
+	/* rotation */
+	quat_to_mat4(mat,q0);
+
+	/* translation */
+	t= dq->trans;
+	mat[3][0]= 2.0f*(-t[0]*q0[1] + t[1]*q0[0] - t[2]*q0[3] + t[3]*q0[2]);
+	mat[3][1]= 2.0f*(-t[0]*q0[2] + t[1]*q0[3] + t[2]*q0[0] - t[3]*q0[1]);
+	mat[3][2]= 2.0f*(-t[0]*q0[3] - t[1]*q0[2] + t[2]*q0[1] + t[3]*q0[0]);
+
+	/* note: this does not handle scaling */
+}	
+
+void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight)
+{
+	int flipped= 0;
+
+	/* make sure we interpolate quats in the right direction */
+	if (dot_qtqt(dq->quat, dqsum->quat) < 0) {
+		flipped= 1;
+		weight= -weight;
+	}
+
+	/* interpolate rotation and translation */
+	dqsum->quat[0] += weight*dq->quat[0];
+	dqsum->quat[1] += weight*dq->quat[1];
+	dqsum->quat[2] += weight*dq->quat[2];
+	dqsum->quat[3] += weight*dq->quat[3];
+
+	dqsum->trans[0] += weight*dq->trans[0];
+	dqsum->trans[1] += weight*dq->trans[1];
+	dqsum->trans[2] += weight*dq->trans[2];
+	dqsum->trans[3] += weight*dq->trans[3];
+
+	/* interpolate scale - but only if needed */
+	if (dq->scale_weight) {
+		float wmat[4][4];
+		
+		if(flipped)	/* we don't want negative weights for scaling */
+			weight= -weight;
+		
+		copy_m4_m4(wmat, dq->scale);
+		mul_m4_fl((float*)wmat, weight);
+		add_m4_m4m4(dqsum->scale, dqsum->scale, wmat);
+		dqsum->scale_weight += weight;
+	}
+}
+
+void normalize_dq(DualQuat *dq, float totweight)
+{
+	float scale= 1.0f/totweight;
+
+	mul_qt_fl(dq->quat, scale);
+	mul_qt_fl(dq->trans, scale);
+	
+	if(dq->scale_weight) {
+		float addweight= totweight - dq->scale_weight;
+		
+		if(addweight) {
+			dq->scale[0][0] += addweight;
+			dq->scale[1][1] += addweight;
+			dq->scale[2][2] += addweight;
+			dq->scale[3][3] += addweight;
+		}
+
+		mul_m4_fl((float*)dq->scale, scale);
+		dq->scale_weight= 1.0f;
+	}
+}
+
+void mul_v3m3_dq(float *co, float mat[][3],DualQuat *dq)
+{	
+	float M[3][3], t[3], scalemat[3][3], len2;
+	float w= dq->quat[0], x= dq->quat[1], y= dq->quat[2], z= dq->quat[3];
+	float t0= dq->trans[0], t1= dq->trans[1], t2= dq->trans[2], t3= dq->trans[3];
+	
+	/* rotation matrix */
+	M[0][0]= w*w + x*x - y*y - z*z;
+	M[1][0]= 2*(x*y - w*z);
+	M[2][0]= 2*(x*z + w*y);
+
+	M[0][1]= 2*(x*y + w*z);
+	M[1][1]= w*w + y*y - x*x - z*z;
+	M[2][1]= 2*(y*z - w*x); 
+
+	M[0][2]= 2*(x*z - w*y);
+	M[1][2]= 2*(y*z + w*x);
+	M[2][2]= w*w + z*z - x*x - y*y;
+	
+	len2= dot_qtqt(dq->quat, dq->quat);
+	if(len2 > 0.0f)
+		len2= 1.0f/len2;
+	
+	/* translation */
+	t[0]= 2*(-t0*x + w*t1 - t2*z + y*t3);
+	t[1]= 2*(-t0*y + t1*z - x*t3 + w*t2);
+	t[2]= 2*(-t0*z + x*t2 + w*t3 - t1*y);
+
+	/* apply scaling */
+	if(dq->scale_weight)
+		mul_m4_v3(dq->scale, co);
+	
+	/* apply rotation and translation */
+	mul_m3_v3(M, co);
+	co[0]= (co[0] + t[0])*len2;
+	co[1]= (co[1] + t[1])*len2;
+	co[2]= (co[2] + t[2])*len2;
+
+	/* compute crazyspace correction mat */
+	if(mat) {
+		if(dq->scale_weight) {
+			copy_m3_m4(scalemat, dq->scale);
+			mul_m3_m3m3(mat, M, scalemat);
+		}
+		else
+			copy_m3_m3(mat, M);
+		mul_m3_fl((float*)mat, len2);
+	}
+}
+
+void copy_dq_dq(DualQuat *dq1, DualQuat *dq2)
+{
+	memcpy(dq1, dq2, sizeof(DualQuat));
+}
+
diff --git a/source/blender/blenlib/intern/math_vector.c b/source/blender/blenlib/intern/math_vector.c
new file mode 100644
index 00000000000..8336b529da3
--- /dev/null
+++ b/source/blender/blenlib/intern/math_vector.c
@@ -0,0 +1,535 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ 
+ * The Original Code is: some of this file.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ * */
+
+#include <float.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "BLI_math.h"
+
+/********************************** Init *************************************/
+
+void zero_v2(float r[2])
+{
+	r[0]= 0.0f;
+	r[1]= 0.0f;
+}
+
+void zero_v3(float r[3])
+{
+	r[0]= 0.0f;
+	r[1]= 0.0f;
+	r[2]= 0.0f;
+}
+
+void copy_v2_v2(float r[2], float a[2])
+{
+	r[0]= a[0];
+	r[1]= a[1];
+}
+
+void copy_v3_v3(float r[3], float a[3])
+{
+	r[0]= a[0];
+	r[1]= a[1];
+	r[2]= a[2];
+}
+
+/********************************* Arithmetic ********************************/
+
+void add_v2_v2(float *r, float *a)
+{
+	r[0] += a[0];
+	r[1] += a[1];
+}
+
+void add_v2_v2v2(float *r, float *a, float *b)
+{
+	r[0]= a[0] + b[0];
+	r[1]= a[1] + b[1];
+}
+
+void add_v3_v3(float *r, float *a)
+{
+	r[0] += a[0];
+	r[1] += a[1];
+	r[1] += a[1];
+}
+
+void add_v3_v3v3(float *r, float *a, float *b)
+{
+	r[0]= a[0] + b[0];
+	r[1]= a[1] + b[1];
+	r[2]= a[2] + b[2];
+}
+
+void sub_v2_v2(float *r, float *a)
+{
+	r[0] -= a[0];
+	r[1] -= a[1];
+}
+
+void sub_v2_v2v2(float *r, float *a, float *b)
+{
+	r[0]= a[0] - b[0];
+	r[1]= a[1] - b[1];
+}
+
+void sub_v3_v3(float *r, float *a)
+{
+	r[0] -= a[0];
+	r[1] -= a[1];
+	r[1] -= a[1];
+}
+
+void sub_v3_v3v3(float *r, float *a, float *b)
+{
+	r[0]= a[0] - b[0];
+	r[1]= a[1] - b[1];
+	r[2]= a[2] - b[2];
+}
+
+void mul_v2_fl(float *v1, float f)
+{
+	v1[0]*= f;
+	v1[1]*= f;
+}
+
+void mul_v3_fl(float r[3], float f)
+{
+	r[0] *= f;
+	r[1] *= f;
+	r[2] *= f;
+}
+
+void mul_v3_v3fl(float r[3], float a[3], float f)
+{
+	r[0]= a[0]*f;
+	r[1]= a[1]*f;
+	r[2]= a[2]*f;
+}
+
+void mul_v3_v3(float r[3], float a[3])
+{
+	r[0] *= a[0];
+	r[1] *= a[1];
+	r[2] *= a[2];
+}
+
+void mul_v3_v3v3(float *v, float *v1, float *v2)
+{
+	v[0] = v1[0] * v2[0];
+	v[1] = v1[1] * v2[1];
+	v[2] = v1[2] * v2[2];
+}
+
+void negate_v3(float r[3])
+{
+	r[0]= -r[0];
+	r[1]= -r[1];
+	r[2]= -r[2];
+}
+
+void negate_v3_v3(float r[3], float a[3])
+{
+	r[0]= -a[0];
+	r[1]= -a[1];
+	r[2]= -a[2];
+}
+
+float dot_v2v2(float *a, float *b)
+{
+	return a[0]*b[0] + a[1]*b[1];
+}
+
+float dot_v3v3(float a[3], float b[3])
+{
+	return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
+}
+
+void cross_v3_v3v3(float r[3], float a[3], float b[3])
+{
+	r[0]= a[1]*b[2] - a[2]*b[1];
+	r[1]= a[2]*b[0] - a[0]*b[2];
+	r[2]= a[0]*b[1] - a[1]*b[0];
+}
+
+void star_m3_v3(float mat[][3], float *vec)
+{
+	mat[0][0]= mat[1][1]= mat[2][2]= 0.0;
+	mat[0][1]= -vec[2];	
+	mat[0][2]= vec[1];
+	mat[1][0]= vec[2];	
+	mat[1][2]= -vec[0];
+	mat[2][0]= -vec[1];	
+	mat[2][1]= vec[0];
+	
+}
+
+/*********************************** Length **********************************/
+
+float len_v2(float *v)
+{
+	return (float)sqrt(v[0]*v[0] + v[1]*v[1]);
+}
+
+float len_v2v2(float *v1, float *v2)
+{
+	float x, y;
+
+	x = v1[0]-v2[0];
+	y = v1[1]-v2[1];
+	return (float)sqrt(x*x+y*y);
+}
+
+float len_v3(float a[3])
+{
+	return sqrtf(dot_v3v3(a, a));
+}
+
+float len_v3v3(float a[3], float b[3])
+{
+	float d[3];
+
+	sub_v3_v3v3(d, b, a);
+	return len_v3(d);
+}
+
+float normalize_v2(float n[2])
+{
+	float d;
+	
+	d= n[0]*n[0]+n[1]*n[1];
+
+	if(d>1.0e-35f) {
+		d= (float)sqrt(d);
+		n[0]/=d; 
+		n[1]/=d; 
+	} else {
+		n[0]=n[1]= 0.0f;
+		d= 0.0f;
+	}
+	return d;
+}
+
+float normalize_v3(float n[3])
+{
+	float d= dot_v3v3(n, n);
+
+	/* a larger value causes normalize errors in a
+	   scaled down models with camera xtreme close */
+	if(d > 1.0e-35f) {
+		d= sqrtf(d);
+		mul_v3_fl(n, 1.0f/d);
+	}
+	else {
+		zero_v3(n);
+		d= 0.0f;
+	}
+
+	return d;
+}
+
+/******************************* Interpolation *******************************/
+
+void interp_v2_v2v2(float *target, const float *a, const float *b, const float t)
+{
+	const float s = 1.0f-t;
+
+	target[0]= s*a[0] + t*b[0];
+	target[1]= s*a[1] + t*b[1];
+}
+
+/* weight 3 2D vectors,
+ * 'w' must be unit length but is not a vector, just 3 weights */
+void interp_v2_v2v2v2(float p[2], const float v1[2], const float v2[2], const float v3[2], const float w[3])
+{
+	p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2];
+	p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2];
+}
+
+
+
+void interp_v3_v3v3(float *target, const float *a, const float *b, const float t)
+{
+	const float s = 1.0f-t;
+
+	target[0]= s*a[0] + t*b[0];
+	target[1]= s*a[1] + t*b[1];
+	target[2]= s*a[2] + t*b[2];
+}
+
+/* weight 3 vectors,
+ * 'w' must be unit length but is not a vector, just 3 weights */
+void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3])
+{
+	p[0] = v1[0]*w[0] + v2[0]*w[1] + v3[0]*w[2];
+	p[1] = v1[1]*w[0] + v2[1]*w[1] + v3[1]*w[2];
+	p[2] = v1[2]*w[0] + v2[2]*w[1] + v3[2]*w[2];
+}
+
+void mid_v3_v3v3(float *v, float *v1, float *v2)
+{
+	v[0]= 0.5f*(v1[0]+ v2[0]);
+	v[1]= 0.5f*(v1[1]+ v2[1]);
+	v[2]= 0.5f*(v1[2]+ v2[2]);
+}
+
+/********************************* Comparison ********************************/
+
+int is_zero_v3(float *v)
+{
+	return (v[0] == 0 && v[1] == 0 && v[2] == 0);
+}
+
+int equals_v3v3(float *v1, float *v2)
+{
+	return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]));
+}
+
+int compare_v3v3(float *v1, float *v2, float limit)
+{
+	if(fabs(v1[0]-v2[0])<limit)
+		if(fabs(v1[1]-v2[1])<limit)
+			if(fabs(v1[2]-v2[2])<limit)
+				return 1;
+
+	return 0;
+}
+
+int compare_len_v3v3(float *v1, float *v2, float limit)
+{
+    float x,y,z;
+
+	x=v1[0]-v2[0];
+	y=v1[1]-v2[1];
+	z=v1[2]-v2[2];
+
+	return ((x*x + y*y + z*z) < (limit*limit));
+}
+
+int compare_v4v4(float *v1, float *v2, float limit)
+{
+	if(fabs(v1[0]-v2[0])<limit)
+		if(fabs(v1[1]-v2[1])<limit)
+			if(fabs(v1[2]-v2[2])<limit)
+				if(fabs(v1[3]-v2[3])<limit)
+					return 1;
+
+	return 0;
+}
+
+/********************************** Angles ***********************************/
+
+/* Return the angle in radians between vecs 1-2 and 2-3 in radians
+   If v1 is a shoulder, v2 is the elbow and v3 is the hand,
+   this would return the angle at the elbow */
+float angle_v3v3v3(float *v1, float *v2, float *v3)
+{
+	float vec1[3], vec2[3];
+
+	sub_v3_v3v3(vec1, v2, v1);
+	sub_v3_v3v3(vec2, v2, v3);
+	normalize_v3(vec1);
+	normalize_v3(vec2);
+
+	return angle_normalized_v3v3(vec1, vec2);
+}
+
+float angle_v2v2v2(float *v1, float *v2, float *v3)
+{
+	float vec1[2], vec2[2];
+
+	vec1[0] = v2[0]-v1[0];
+	vec1[1] = v2[1]-v1[1];
+	
+	vec2[0] = v2[0]-v3[0];
+	vec2[1] = v2[1]-v3[1];
+	
+	normalize_v2(vec1);
+	normalize_v2(vec2);
+
+	return angle_normalized_v2v2(vec1, vec2);
+}
+
+/* Return the shortest angle in radians between the 2 vectors */
+float angle_v2v2(float *v1, float *v2)
+{
+	float vec1[3], vec2[3];
+
+	copy_v3_v3(vec1, v1);
+	copy_v3_v3(vec2, v2);
+	normalize_v3(vec1);
+	normalize_v3(vec2);
+
+	return angle_normalized_v3v3(vec1, vec2);
+}
+
+float angle_normalized_v3v3(float *v1, float *v2)
+{
+	/* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
+	if (dot_v3v3(v1, v2) < 0.0f) {
+		float vec[3];
+		
+		vec[0]= -v2[0];
+		vec[1]= -v2[1];
+		vec[2]= -v2[2];
+		
+		return (float)M_PI - 2.0f*(float)saasin(len_v3v3(vec, v1)/2.0f);
+	}
+	else
+		return 2.0f*(float)saasin(len_v3v3(v2, v1)/2.0f);
+}
+
+float angle_normalized_v2v2(float *v1, float *v2)
+{
+	/* this is the same as acos(dot_v3v3(v1, v2)), but more accurate */
+	if (dot_v2v2(v1, v2) < 0.0f) {
+		float vec[2];
+		
+		vec[0]= -v2[0];
+		vec[1]= -v2[1];
+		
+		return (float)M_PI - 2.0f*saasin(len_v2v2(vec, v1)/2.0f);
+	}
+	else
+		return 2.0f*(float)saasin(len_v2v2(v2, v1)/2.0f);
+}
+
+/********************************* Geometry **********************************/
+
+/* Project v1 on v2 */
+void project_v3_v3v3(float *c, float *v1, float *v2)
+{
+	float mul;
+	mul = dot_v3v3(v1, v2) / dot_v3v3(v2, v2);
+	
+	c[0] = mul * v2[0];
+	c[1] = mul * v2[1];
+	c[2] = mul * v2[2];
+}
+
+/* Returns a vector bisecting the angle at v2 formed by v1, v2 and v3 */
+void bisect_v3_v3v3v3(float *out, float *v1, float *v2, float *v3)
+{
+	float d_12[3], d_23[3];
+	sub_v3_v3v3(d_12, v2, v1);
+	sub_v3_v3v3(d_23, v3, v2);
+	normalize_v3(d_12);
+	normalize_v3(d_23);
+	add_v3_v3v3(out, d_12, d_23);
+	normalize_v3(out);
+}
+
+/* Returns a reflection vector from a vector and a normal vector
+reflect = vec - ((2 * DotVecs(vec, mirror)) * mirror)
+*/
+void reflect_v3_v3v3(float *out, float *v1, float *v2)
+{
+	float vec[3], normal[3];
+	float reflect[3] = {0.0f, 0.0f, 0.0f};
+	float dot2;
+
+	copy_v3_v3(vec, v1);
+	copy_v3_v3(normal, v2);
+
+	normalize_v3(normal);
+
+	dot2 = 2 * dot_v3v3(vec, normal);
+
+	reflect[0] = vec[0] - (dot2 * normal[0]);
+	reflect[1] = vec[1] - (dot2 * normal[1]);
+	reflect[2] = vec[2] - (dot2 * normal[2]);
+
+	copy_v3_v3(out, reflect);
+}
+
+void ortho_basis_v3v3_v3(float *v1, float *v2, float *v)
+{
+	const float f = (float)sqrt(v[0]*v[0] + v[1]*v[1]);
+
+	if (f < 1e-35f) {
+		// degenerate case
+		v1[0] = (v[2] < 0.0f) ? -1.0f : 1.0f;
+		v1[1] = v1[2] = v2[0] = v2[2] = 0.0f;
+		v2[1] = 1.0f;
+	}
+	else  {
+		const float d= 1.0f/f;
+
+		v1[0] =  v[1]*d;
+		v1[1] = -v[0]*d;
+		v1[2] = 0.0f;
+		v2[0] = -v[2]*v1[1];
+		v2[1] = v[2]*v1[0];
+		v2[2] = v[0]*v1[1] - v[1]*v1[0];
+	}
+}
+
+/*********************************** Other ***********************************/
+
+void print_v2(char *str, float v[2])
+{
+	printf("%s: %.3f %.3f\n", str, v[0], v[1]);
+}
+
+void print_v3(char *str, float v[3])
+{
+	printf("%s: %.3f %.3f %.3f\n", str, v[0], v[1], v[2]);
+}
+
+void print_v4(char *str, float v[4])
+{
+	printf("%s: %.3f %.3f %.3f %.3f\n", str, v[0], v[1], v[2], v[3]);
+}
+
+void normal_short_to_float_v3(float *out, short *in)
+{
+	out[0] = in[0]*(1.0f/32767.0f);
+	out[1] = in[1]*(1.0f/32767.0f);
+	out[2] = in[2]*(1.0f/32767.0f);
+}
+
+void normal_float_to_short_v3(short *out, float *in)
+{
+	out[0] = (short)(in[0]*32767.0f);
+	out[1] = (short)(in[1]*32767.0f);
+	out[2] = (short)(in[2]*32767.0f);
+}
+
+void minmax_v3_v3v3(float *min, float *max, float *vec)
+{
+	if(min[0]>vec[0]) min[0]= vec[0];
+	if(min[1]>vec[1]) min[1]= vec[1];
+	if(min[2]>vec[2]) min[2]= vec[2];
+
+	if(max[0]<vec[0]) max[0]= vec[0];
+	if(max[1]<vec[1]) max[1]= vec[1];
+	if(max[2]<vec[2]) max[2]= vec[2];
+}
+