#include <assert.h>
#include <gsl/gsl_nan.h>
#include "csm_all.h"

Include dependency graph for math_utils.c:

Functions
double	angleDiff (double a, double b)
int	any_nan (const double *d, int n)
void	copy_d (const double from, int n, double to)
int	count_equal (const int *v, int n, int value)
double	deg2rad (double deg)
double	dist_to_segment_d (const double a[2], const double b[2], const double x[2])
double	dist_to_segment_squared_d (const double a[2], const double b[2], const double x[2])
double	distance_d (const double a[2], const double b[2])
double	distance_squared_d (const double a[2], const double b[2])
double	dot_d (const double p[2], const double q[2])
const char *	friendly_pose (const double *pose)
int	is_nan (double v)
double	max_in_array (const double *v, int n)
int	minmax (int from, int to, int x)
double	norm_d (const double p[2])
double	normalize_0_2PI (double t)
void	ominus_d (const double x[3], double res[3])
void	oplus_d (const double x1[3], const double x2[3], double res[3])
void	pose_diff_d (const double pose2[3], const double pose1[3], double res[3])
void	possible_interval (const double p_i_w, LDP ld, double max_angular_correction_deg, double max_linear_correction, int from, int to, int start_cell)
void	projection_on_line_d (const double a[2], const double b[2], const double p[2], double res[2], double *distance)
void	projection_on_segment_d (const double a[2], const double b[2], const double x[2], double proj[2])
double	rad2deg (double rad)
double	segment_alpha (const double p0[2], const double p1[2])
int	segment_ray_tracing (const double p0[2], const double p1[2], const double eye[2], double direction, double *range)
double	square (double x)
void	transform_d (const double point2d[2], const double pose[3], double result2d[2])
Variables
int	distance_counter = 0
static char	tmp_buf [1024]

Function Documentation

double angleDiff	(	double	a,
		double	b
	)

Returns an angle difference in the [-pi, pi] range

Definition at line 128 of file math_utils.c.

int any_nan	(	const double *	d,
		int	n
	)

Returns true if any value in d is NAN

Definition at line 64 of file math_utils.c.

void copy_d	(	const double *	from,
		int	n,
		double *	to
	)

Copies n doubles from from to to

Definition at line 83 of file math_utils.c.

int count_equal	(	const int *	v,
		int	n,
		int	value
	)

Count numbers of items in array v equal to value

Definition at line 192 of file math_utils.c.

double deg2rad ( double deg )

Degrees to radians

Definition at line 75 of file math_utils.c.

double dist_to_segment_d	(	const double	a[2],
		const double	b[2],
		const double	x[2]
	)

Distance of x from its projection on segment a-b

Definition at line 181 of file math_utils.c.

double dist_to_segment_squared_d	(	const double	a[2],
		const double	b[2],
		const double	x[2]
	)

Same thing as dist_to_segment_d(), but squared

Definition at line 174 of file math_utils.c.

double distance_d	(	const double	a[2],
		const double	b[2]
	)

Definition at line 55 of file math_utils.c.

double distance_squared_d	(	const double	a[2],
		const double	b[2]
	)

Definition at line 48 of file math_utils.c.

double dot_d	(	const double	p[2],
		const double	q[2]
	)

Definition at line 211 of file math_utils.c.

const char* friendly_pose ( const double * pose )

A function to print poses and covariances in a friendly way

Definition at line 266 of file math_utils.c.

int is_nan ( double v )

Returns true v is NAN

Definition at line 60 of file math_utils.c.

double max_in_array	(	const double *	v,
		int	n
	)

Maximum value in the array

Definition at line 273 of file math_utils.c.

int minmax	(	int	from,
		int	to,
		int	x
	)

Definition at line 6 of file math_utils.c.

double norm_d ( const double p[2] )

Returns norm of 2D point p

Definition at line 71 of file math_utils.c.

double normalize_0_2PI ( double angle )

Normalizes an angle in the 0-2PI range

Definition at line 198 of file math_utils.c.

void ominus_d	(	const double	x[3],
		double	res[3]
	)

These are the operators defined in Smith & Cheeseman

Definition at line 87 of file math_utils.c.

void oplus_d	(	const double	x1[3],
		const double	x2[3],
		double	res[3]
	)

safe if res == x1

Definition at line 96 of file math_utils.c.

void pose_diff_d	(	const double	pose2[3],
		const double	pose1[3],
		double	res[3]
	)

Definition at line 115 of file math_utils.c.

void possible_interval	(	const double *	p_i_w,
		LDP	ld,
		double	max_angular_correction_deg,
		double	max_linear_correction,
		int *	from,
		int *	to,
		int *	start_cell
	)

Definition at line 10 of file math_utils.c.

void projection_on_line_d	(	const double	a[2],
		const double	b[2],
		const double	p[2],
		double	res[2],
		double *	distance
	)

Projects (p[0],p[1]) on the LINE passing through (ax,ay)-(bx,by). If distance!=0, distance is set to the distance from the point to the segment

Definition at line 135 of file math_utils.c.

void projection_on_segment_d	(	const double	a[2],
		const double	b[2],
		const double	P[2],
		double	proj[2]
	)

Projection of P on the SEGMENT A-B

Definition at line 156 of file math_utils.c.

double rad2deg ( double rad )

Radians to degrees

Definition at line 79 of file math_utils.c.

double segment_alpha	(	const double	p0[2],
		const double	p1[2]
	)

Returns the orientation of the normal for the line passing through p0-p1

Definition at line 257 of file math_utils.c.

int segment_ray_tracing	(	const double	p0[2],
		const double	p1[2],
		const double	eye[2],
		double	direction,
		double *	range
	)

Definition at line 218 of file math_utils.c.

double square ( double x )

Definition at line 124 of file math_utils.c.

void transform_d	(	const double	point2d[2],
		const double	pose[3],
		double	result2d[2]
	)

Definition at line 108 of file math_utils.c.

Variable Documentation

int distance_counter = 0

Definition at line 46 of file math_utils.c.

char tmp_buf[1024] [static]

Definition at line 265 of file math_utils.c.

Functions

Variables

Function Documentation

Variable Documentation