total.h File Reference

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions
tfloat	triangulationPierlot (tfloat *x, tfloat *y, tfloat alpha1, tfloat alpha2, tfloat alpha3, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)
tfloat	triangulationPierlot2 (tfloat *x, tfloat *y, tfloat alpha1, tfloat alpha2, tfloat alpha3, tfloat x1, tfloat y1, tfloat x2, tfloat y2, tfloat x3, tfloat y3)

Function Documentation

tfloat triangulationPierlot	(	tfloat *	x,
		tfloat *	y,
		tfloat	alpha1,
		tfloat	alpha2,
		tfloat	alpha3,
		tfloat	x1,
		tfloat	y1,
		tfloat	x2,
		tfloat	y2,
		tfloat	x3,
		tfloat	y3
	)

Implementation of the ToTal algorithm

Parameters

x	Horizontal position of the robot
y	Vertical position of the robot
alpha1	Angle with beacon 1
alpha2	Angle with beacon 2
alpha3	Angle with beacon 3
x1	Horizontal position of beacon 1
y1	Vertical position of beacon 1
x2	Horizontal position of beacon 2
y2	Vertical position of beacon 2
x3	Horizontal position of beacon 3
y3	Vertical position of beacon 3

Returns

??

Definition at line 16 of file total.c.

References PI.

Referenced by triangulationMethod().

{
    tfloat cot_12 = Cot( alpha2 - alpha1 ) ;
    tfloat cot_23 = Cot( alpha3 - alpha2 ) ;
    cot_12 = adjust_value_to_bounds( cot_12 , COT_MAX ) ;
    cot_23 = adjust_value_to_bounds( cot_23 , COT_MAX ) ;
    tfloat cot_31 = ( 1.0 - cot_12 * cot_23 ) / ( cot_12 + cot_23 ) ;
    cot_31 = adjust_value_to_bounds( cot_31 , COT_MAX ) ;
    
    tfloat x1_ = x1 - x2 , y1_ = y1 - y2 , x3_ = x3 - x2 , y3_ = y3 - y2 ;

    tfloat c12x = x1_ + cot_12 * y1_ ;
    tfloat c12y = y1_ - cot_12 * x1_ ;

    tfloat c23x = x3_ - cot_23 * y3_ ;
    tfloat c23y = y3_ + cot_23 * x3_ ;

    tfloat c31x = (x3_ + x1_) + cot_31 * (y3_ - y1_) ;
    tfloat c31y = (y3_ + y1_) - cot_31 * (x3_ - x1_) ;
    tfloat k31 = (x3_ * x1_) + (y3_ * y1_) + cot_31 * ( (y3_ * x1_) - (x3_ * y1_) ) ;
  
  tfloat D = (c12x - c23x) * (c23y - c31y) - (c23x - c31x) * (c12y - c23y) ;
  tfloat invD = 1.0 / D ;
  tfloat K = k31 * invD ;
  
    *x = K * (c12y - c23y) + x2 ;
    *y = K * (c23x - c12x) + y2 ;
    
    return invD ; /* return 1/D */
}