geom.cpp File Reference

#include "geom.h"
#include "stdlib.h"
#include <stdio.h>
#include <math.h>
#include <assert.h>

Namespaces
namespace	geom
Defines
#define	M_E   2.7182818284590452354
#define	M_LOG2E   1.4426950408889634074
#define	M_LOG10E   0.43429448190325182765
#define	M_LN2   0.69314718055994530942
#define	M_LN10   2.30258509299404568402
#define	M_PI   3.14159265358979323846
#define	M_PI_2   1.57079632679489661923
#define	M_PI_4   0.78539816339744830962
#define	M_1_PI   0.31830988618379067154
#define	M_2_PI   0.63661977236758134308
#define	M_2_SQRTPI   1.12837916709551257390
#define	M_SQRT2   1.41421356237309504880
#define	M_SQRT1_2   0.70710678118654752440
#define	fsin   sin
#define	fcos   cos
#define	ftan   tan
#define	fasin   asin
#define	facos   acos
#define	fatan   atan
#define	fatan2   atan2
#define	fsqrt   sqrt
#define	DEBUG_GEOM   false
#define	SEUIL_GREV1   0.01
#define	BIG_W   1.0E+19
#define	W_MIN_OVER_W_MAX   0.0001
#define	W_PREC   1.0E-10
#define	matrix_is_affine(m)
#define	EPSILON   1e-4
#define	MAX(a, b)   (((a)>(b))?(a):(b))
#define	GEOM_SIGN(a, b)   ((b) >= 0.0 ? fabs(a) : -fabs(a))
#define	GEOM_MIN(a, b)   ((a) < (b) ? (a) : (b))
#define	EQN_EPS   1e-9
#define	IsZero(x)   ((x) > -EQN_EPS && (x) < EQN_EPS)
#define	CBRT(x)
Functions
static void	nrerror (char *error_text)
static double **	matmxn_convert (double *a, int nrl, int nrh, int ncl, int nch)
static void	Jinv (double *w, int n, double damping)
static void	svd (double *a, int m, int n, double *w, double *v)
static double	pythag (double a, double b)
double	geom::safe_acos (double f)
double	geom::safe_asin (double f)
void	geom::matrix_idt_ (MatriX matrix)
void	geom::matrix_copy (const MatriX src, MatriX dst)
void	geom::matrix_compose (const MR3D mr, const VT3D vt, MatriX matrix)
void	geom::matrix_decompose (MR3D mr, VT3D vt, const MatriX matrix)
void	geom::matrix_transpose (const MatriX a, MatriX at)
void	geom::matrix_mult_vhom (const MatriX a, const VHOM u, VHOM v)
void	geom::matrix_mult_vt3d (const MatriX ma, const VT3D vu, VT3D vv)
void	geom::matrix_inverse (const MatriX mdir, MatriX minv)
void	geom::matrix_inverse_det (MatriX res, LIBGEOM_REAL determinant, const MatriX source)
LIBGEOM_REAL	geom::matrix_cofactor (const MatriX matrix, int i, int j)
LIBGEOM_REAL	geom::matrix_determinant (const MatriX matrix)
void	geom::matrix_mult (const MatriX ma, const MatriX mb, MatriX mc)
bool	geom::matrix_egalite (const MatriX a, const MatriX b)
int	geom::matrix_attraction_idt (MatriX a)
int	geom::matrix_proximite (const MatriX a, const MatriX b, LIBGEOM_REAL seuil_mr, LIBGEOM_REAL seuil_vt)
bool	geom::matrix_write (const MatriX m, FILE *f)
bool	geom::matrix_read (MatriX m, FILE *f)
void	geom::mr3d_idt_ (MR3D mr)
void	geom::mr3d_get (const MatriX matrix, MR3D a)
void	geom::mr3d_copy (const MR3D src, MR3D dst)
void	geom::mr3d_load (MatriX matrix, const MR3D a)
void	geom::mr3d_transpose (const MR3D ma, MR3D mat)
void	geom::mr3d_self_transpose (MR3D ma)
void	geom::mr3d_perm_circ (MR3D a, MR3D b)
void	geom::mr3d_opposite (MR3D a, MR3D b)
void	geom::mr3d_symtxy (MR3D a, MR3D b)
void	geom::mr3d_scale (const MR3D self, LIBGEOM_REAL scalar, MR3D res)
void	geom::mr3d_sub (const MR3D left, const MR3D right, MR3D res)
void	geom::mr3d_add (const MR3D left, const MR3D right, MR3D res)
void	geom::mr3d_mult (const MR3D a, const MR3D b, MR3D c)
void	geom::mr3d_mult_vt3d (const MR3D ma, const VT3D vu, VT3D vv)
void	geom::mr3d_from_axis_angle (MR3D mr, const VT3D faxis)
void	geom::mr3d_to_axis_angle (VT3D axis, const MR3D mr)
double	geom::mr3d_axis_angle (const MR3D mr)
bool	geom::mr3d_egalite (const MR3D a, const MR3D b, LIBGEOM_REAL seuil_mr)
int	geom::mr3d_attraction_idt (MR3D a, LIBGEOM_REAL seuil_mr)
int	geom::mr3d_attraction_ini (MR3D a, LIBGEOM_REAL seuil_mr)
void	geom::mr3d_conv_xyz_mob (const MR3D m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, LIBGEOM_REAL *ang3)
void	geom::mr3d_conv_yxz_mob (const MR3D m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, LIBGEOM_REAL *ang3)
void	geom::mr3d_conv_EulerBoulic_zxy (MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, LIBGEOM_REAL *ang3, bool verbose)
void	geom::mr3d_conv_EulerBoulic_zyx (MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, LIBGEOM_REAL *ang3, bool verbose)
void	geom::mr3d_conv_EulerBoulic_zx (MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, bool verbose)
void	geom::mr3d_conv_EulerBoulic_zy (MR3D_t m, LIBGEOM_REAL *ang1, LIBGEOM_REAL *ang2, bool verbose)
void	geom::mr3d_conv_EulerBoulic_z (MR3D_t m, LIBGEOM_REAL *ang1, bool verbose)
void	geom::mr3d_reorthogonalize (MR3D R, int limit)
bool	geom::mr3d_write (const MR3D self, FILE *f)
bool	geom::mr3d_read (MR3D self, FILE *f)
void	geom::vt3d_get (const MatriX matrix, VT3D v)
void	geom::vt3d_load (MatriX matrix, const VT3D v)
void	geom::vt3d_vhom (const VT3D vt3d, VHOM vhom)
void	geom::vhom_vt3d (const VHOM vhom, VT3D vt3d)
void	geom::vt3d_let (LIBGEOM_REAL x, LIBGEOM_REAL y, LIBGEOM_REAL z, VT3D w)
void	geom::vt3d_copy (const VT3D src, VT3D dst)
void	geom::vt3d_set_null (VT3D dst)
void	geom::vt3d_add (const VT3D u, const VT3D v, VT3D w)
void	geom::vt3d_sub (const VT3D u, const VT3D v, VT3D w)
void	geom::vt3d_mult (const VT3D u, const VT3D v, VT3D w)
void	geom::vt3d_div (const VT3D u, const VT3D v, VT3D w)
void	geom::vt3d_mults (const VT3D v, LIBGEOM_REAL s, VT3D w)
void	geom::vt3d_divs (const VT3D v, LIBGEOM_REAL s, VT3D w)
void	geom::vt3d_neg (const VT3D src, VT3D dest)
LIBGEOM_REAL	geom::vt3d_get_norm (const VT3D v)
LIBGEOM_REAL	geom::vt3d_get_norm2 (const VT3D v)
LIBGEOM_REAL	geom::vt3d_get_dist (const VT3D u, const VT3D v)
LIBGEOM_REAL	geom::vt3d_get_distptline (const VT3D p, const VT3D q, const VT3D n)
LIBGEOM_REAL	geom::vt3d_get_distptplane (const VT3D p, const VT3D q, const VT3D n)
void	geom::vt3d_rotv (const VT3D v, LIBGEOM_REAL sa, LIBGEOM_REAL ca, char k, VT3D w)
void	geom::vt3d_rotvect (const VT3D v, const VT3D c, VT3D k, LIBGEOM_REAL sa, LIBGEOM_REAL ca, VT3D w)
bool	geom::vt3d_normalize (const VT3D u, VT3D v)
void	geom::vt3d_cross (const VT3D u, const VT3D v, VT3D w)
void	geom::vt3d_scale (const VT3D scale, const VT3D u, VT3D v)
LIBGEOM_REAL	geom::vt3d_get_dot (const VT3D u, const VT3D v)
bool	geom::vt3d_egalite (const VT3D a, const VT3D b, LIBGEOM_REAL seuil_vt)
int	geom::vt3d_attraction_nul (VT3D a, LIBGEOM_REAL seuil_vt)
bool	geom::vt3d_write (const VT3D self, FILE *f)
bool	geom::vt3d_read (VT3D self, FILE *f)
static void	geom::svd_invert (double *target, double *v, double *w, double *J, int width, int height, bool transpose)
void	geom::matmxn_compute_LS_and_DLS_inverse (double *LS_inv, double *DLS_inv, double *matrix, int width, int height, double *task, double damping)
void	geom::matrix_rotpart (MatriX_t In, MatriX_t Res)
void	geom::matrix_mult_opt (const MatriX ma, const MatriX mb, MatriX mc)
void	geom::matrix_lerp (const MatriX_t a, const MatriX_t b, LIBGEOM_REAL u, MatriX result)
void	geom::mr3d_from_angle_seq_xyz (MR3D mx, LIBGEOM_REAL angX, LIBGEOM_REAL angY, LIBGEOM_REAL angZ)
void	geom::mr3d_from_angle_seq_yxz (MR3D mx, LIBGEOM_REAL angY, LIBGEOM_REAL angX, LIBGEOM_REAL angZ)
void	geom::mr3d_from_two_vectors (const VT3D_t a, const VT3D_t b, MR3D R)
bool	geom::vt3d_inside_polygon (VT3D point, VT3D *p, int N)
void	geom::vt3d_rotate_around_axis_angle (const VT3D axis_angle, const VT3D v_in, VT3D v_out)
void	geom::vt3d_lerp (const VT3D p1, const VT3D p2, LIBGEOM_REAL u, VT3D result)
void	geom::vt3d_slerp (const VT3D p1, const VT3D p2, LIBGEOM_REAL u, VT3D result)
double *	geom::vtn_creation (int n)
void	geom::vtn_destr (double *p)
void	geom::vtn_nul (double *p, int n)
double	geom::vtn_norm (const double *p, int n)
double	geom::vtn_anorm (const double *p, int n)
void	geom::vtn_copy (const double *psrc, double *pdst, int n)
void	geom::vtn_add (const double *p1, const double *p2, double *pdst, int n)
void	geom::vtn_sub (const double *p1, const double *p2, double *pdst, int n)
void	geom::vtn_mults (const double *psrc, double *pdst, int n, double alpha)
double	geom::vtn_dot (const double *p1, const double *p2, int n)
double	geom::vtn_inv (const double *p, double *p_inv, int n)
double *	geom::matmxn_creation (int lmax, int cmax)
void	geom::matmxn_destr (double *p)
void	geom::matmxn_nul (double *p, int lmax, int cmax)
void	geom::matmxn_idt (double *p, int lmax, int cmax)
void	geom::matmxn_copy (const double *a, double *b, int lmax, int cmax)
void	geom::matmxn_mult (const double *a, const double *b, double *c, int imax, int jmax, int kmax)
void	geom::matmxn_mult_vtn (const double *a, const double *b, double *c, int lmax, int cmax)
void	geom::matmxn_write (double *d, int width, int height, FILE *f)
void	geom::matmxn_transpose (const double *src, double *dest, int src_width, int src_height)
bool	geom::matmxn_pseudo_inverse (double *m, int lmax, int cmax, double *mp)
void	geom::matmxn_damped_least_square_inverse (double *j_inv, double *j_mat, int j_width, int j_height, double damping)
void	geom::matmxn_svdcmp (double **a, int m, int n, double *w, double **v)
void	geom::quat_set (QUATERNION self, LIBGEOM_REAL w, LIBGEOM_REAL x, LIBGEOM_REAL y, LIBGEOM_REAL z)
void	geom::quat_set_null (QUATERNION self)
void	geom::quat_set_unit (QUATERNION self)
void	geom::quat_assign (QUATERNION self, const QUATERNION newQuat)
void	geom::quat_copy (const QUATERNION src, QUATERNION dest)
void	geom::quat_get_inv (const QUATERNION self, QUATERNION inv)
void	geom::quat_get_conj (const QUATERNION self, QUATERNION conj)
LIBGEOM_REAL	geom::quat_get_norm (const QUATERNION self)
LIBGEOM_REAL	geom::quat_get_distance (const QUATERNION left, const QUATERNION right)
LIBGEOM_REAL	geom::quat_get_sdistance (const QUATERNION left, const QUATERNION right)
void	geom::quat_neg (QUATERNION self)
void	geom::quat_normalize (QUATERNION self)
LIBGEOM_REAL	geom::quat_dot (const QUATERNION left, const QUATERNION right)
void	geom::quat_pow (const QUATERNION self, LIBGEOM_REAL alpha, QUATERNION result)
void	geom::quat_exp (const QUATERNION self, QUATERNION result)
void	geom::quat_ln (const QUATERNION self, QUATERNION result)
void	geom::quat_mults (const QUATERNION self, LIBGEOM_REAL s, QUATERNION result)
void	geom::quat_mult (const QUATERNION left, const QUATERNION right, QUATERNION result)
void	geom::quat_sub (const QUATERNION left, const QUATERNION right, QUATERNION result)
void	geom::quat_add (const QUATERNION left, const QUATERNION right, QUATERNION result)
void	geom::quat_slerp (const QUATERNION left, const QUATERNION right, LIBGEOM_REAL u, QUATERNION result)
void	geom::quat_dump (const QUATERNION self)
void	geom::quat_from_axis (QUATERNION self, const VT3D axis, LIBGEOM_REAL angle)
void	geom::quat_from_axis_angle (QUATERNION q, const VT3D axis_angle)
void	geom::quat_to_axis_angle (const QUATERNION quat, VT3D axis)
void	geom::quat_from_matrix (const MatriX m, QUATERNION quat)
void	geom::quat_from_mr3d (const MR3D m, QUATERNION quat)
void	geom::quat_to_matrix (const QUATERNION self, MatriX result)
void	geom::quat_to_mr3d (const QUATERNION self, MR3D result)
void	geom::quat_from_two_vectors (const VT3D v1, const VT3D v2, QUATERNION q)
void	geom::quat_from_swing (QUATERNION q, const LIBGEOM_REAL swing[2])
void	geom::quat_from_swing_and_twist (QUATERNION q, const LIBGEOM_REAL swing[2], LIBGEOM_REAL twist)
void	geom::quat_to_swing_and_twist (const QUATERNION q, LIBGEOM_REAL swing[2], LIBGEOM_REAL *twist)
ARRAY1D *	geom::array1d_creation (int lines)
void	geom::array1d_destroy (ARRAY1D *array1d)
void	geom::array1d_nul (ARRAY1D *array1d)
double	geom::array1d_norm (ARRAY1D *array1d)
double	geom::array1d_anorm (ARRAY1D *array1d)
void	geom::array1d_copy (ARRAY1D *src, ARRAY1D *dest)
void	geom::array1d_add (ARRAY1D *src_left, ARRAY1D *src_right, ARRAY1D *dest)
void	geom::array1d_sub (ARRAY1D *src_left, ARRAY1D *src_right, ARRAY1D *dest)
void	geom::array1d_mults (ARRAY1D *src, ARRAY1D *dest, double factor)
double	geom::array1d_dot (ARRAY1D *src1, ARRAY1D *src2)
double	geom::array1d_inv (ARRAY1D *src, ARRAY1D *dest)
void	geom::array1d_write (ARRAY1D *a, FILE *file)
ARRAY2D *	geom::array2d_creation (int lines, int cols)
void	geom::array2d_destroy (ARRAY2D *array2d)
int	geom::array2d_same_size (ARRAY2D *a, ARRAY2D *b)
void	geom::array2d_nul (ARRAY2D *array2d)
void	geom::array2d_idt (ARRAY2D *array2d)
void	geom::array2d_copy (ARRAY2D *src, ARRAY2D *dest)
void	geom::array2d_mult (ARRAY2D *a, ARRAY2D *b, ARRAY2D *dest)
void	geom::array2d_add (ARRAY2D *a, ARRAY2D *b, ARRAY2D *dest)
void	geom::array2d_sub (ARRAY2D *a, ARRAY2D *b, ARRAY2D *dest)
void	geom::array2d_mult_array1d (ARRAY2D *array2d, ARRAY1D *array1d, ARRAY1D *dest)
void	geom::array2d_scale (ARRAY2D *src, float scalar, ARRAY2D *dest)
void	geom::array2d_pseudo_inverse (ARRAY2D *src, ARRAY2D *dest)
void	geom::array2d_damped_least_square_inverse (ARRAY2D *j_inv, ARRAY2D *j_mat, double damping)
void	geom::array2d_compute_LS_and_DLS_inverse (ARRAY2D *LS_inv, ARRAY2D *DLS_inv, ARRAY2D *j_mat, ARRAY1D *task, double damping)
void	geom::array2d_write (ARRAY2D *a, FILE *file)
void	geom::array2d_set_column (ARRAY2D *array2d, int column, double value)
void	geom::array2d_set_line (ARRAY2D *array2d, int line, double value)
void	geom::array2d_transpose (ARRAY2D *src, ARRAY2D *dest)
int	geom::polynomial_solve_quadric (double c[3], double s[2])
int	geom::polynomial_solve_cubic (double c[4], double s[3])
int	geom::polynomial_solve_quartic (double c[5], double s[4])
Variables
const float	geom::EPSIL_MR3D = 0.00001f
const float	geom::EPSIL_VT3D = 0.001f
static LIBGEOM_REAL	geom::coef [10]

---

Define Documentation

#define BIG_W   1.0E+19

#define CBRT

(

x

)

Value:

((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
                          ((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))

#define DEBUG_GEOM   false

#define EPSILON   1e-4

#define EQN_EPS   1e-9

#define facos   acos

#define fasin   asin

#define fatan   atan

#define fatan2   atan2

#define fcos   cos

#define fsin   sin

#define fsqrt   sqrt

#define ftan   tan

#define GEOM_MIN	(	a,
		b		)	((a) < (b) ? (a) : (b))

#define GEOM_SIGN	(	a,
		b		)	((b) >= 0.0 ? fabs(a) : -fabs(a))

#define IsZero

(

x

)

((x) > -EQN_EPS && (x) < EQN_EPS)

#define M_1_PI   0.31830988618379067154

#define M_2_PI   0.63661977236758134308

#define M_2_SQRTPI   1.12837916709551257390

#define M_E   2.7182818284590452354

#define M_LN10   2.30258509299404568402

#define M_LN2   0.69314718055994530942

#define M_LOG10E   0.43429448190325182765

#define M_LOG2E   1.4426950408889634074

#define M_PI   3.14159265358979323846

#define M_PI_2   1.57079632679489661923

#define M_PI_4   0.78539816339744830962

#define M_SQRT1_2   0.70710678118654752440

#define M_SQRT2   1.41421356237309504880

#define matrix_is_affine

(

m

)

Value:

((m [0][3] == 0.) && (m [1][3] == 0.) && \
                             (m [2][3] == 0.) && (m [3][3] == 1.))

#define MAX	(	a,
		b		)	(((a)>(b))?(a):(b))

#define SEUIL_GREV1   0.01

#define W_MIN_OVER_W_MAX   0.0001

#define W_PREC   1.0E-10

---

Function Documentation

void Jinv	(	double *	w,
		int	n,
		double	damping
	)			[static]

static double ** matmxn_convert	(	double *	a,
		int	nrl,
		int	nrh,
		int	ncl,
		int	nch
	)			[static]

static void nrerror

(

char *

error_text

)

[static]

static double pythag	(	double	a,
		double	b
	)			[static]

static void svd	(	double *	a,
		int	m,
		int	n,
		double *	w,
		double *	v
	)			[static]

---