1 #include <iostream>
2 #include <cmath>
3 #include "rsdl_dist.h"
4
5 #include <cassert>
6 #ifdef _MSC_VER
7 # include "vcl_msvc_warnings.h"
8 #endif
9 #include "vnl/vnl_math.h"
10
11 double
rsdl_dist_sq(const rsdl_point & p,const rsdl_point & q)12 rsdl_dist_sq( const rsdl_point& p, const rsdl_point& q )
13 {
14 unsigned int Nc = p.num_cartesian();
15 unsigned int Na = p.num_angular();
16 assert( Nc == q.num_cartesian() && Na == q.num_angular() );
17
18 double sum_sq = 0;
19
20 for ( unsigned int i=0; i<Nc; ++i ) {
21 sum_sq += vnl_math::sqr( p.cartesian(i) - q.cartesian(i) );
22 }
23
24 for ( unsigned int j=0; j<Na; ++j ) {
25 double diff = vnl_math::abs( p.angular(j) - q.angular(j) );
26 if ( diff > vnl_math::pi ) {
27 diff = vnl_math::twopi - diff;
28 }
29 sum_sq += vnl_math::sqr( diff );
30 }
31
32 return sum_sq;
33 }
34
35
36 double
rsdl_dist(const rsdl_point & p,const rsdl_point & q)37 rsdl_dist( const rsdl_point& p, const rsdl_point& q )
38 {
39 return std::sqrt( rsdl_dist_sq( p, q ) );
40 }
41
42
43 double
rsdl_dist_sq(const rsdl_point & p,const rsdl_bounding_box & b)44 rsdl_dist_sq( const rsdl_point & p, const rsdl_bounding_box & b )
45 {
46 double sum_sq = 0;
47 unsigned int Nc = p.num_cartesian();
48 unsigned int Na = p.num_angular();
49 assert( Nc == b.num_cartesian() && Na == b.num_angular() );
50
51 for ( unsigned int i=0; i<Nc; ++i ) {
52 double x0 = b.min_cartesian(i), x1 = b.max_cartesian(i);
53 double x = p.cartesian(i);
54 if ( x < x0 ) {
55 sum_sq += vnl_math::sqr( x0 - x );
56 }
57 else if ( x > x1 ) {
58 sum_sq += vnl_math::sqr( x1 - x );
59 }
60 }
61
62 for ( unsigned int j=0; j<Na; ++j ) {
63 double a0 = b.min_angular(j), a1 = b.max_angular(j);
64 double a = p.angular(j);
65 if ( a0 > a1 ) { // interval wraps around 0
66 if ( a < a0 && a > a1 ) { // outside interval, calculate distance
67 sum_sq += vnl_math::sqr( std::min( a0-a, a-a1 ) );
68 }
69 }
70 else { // interval does not wrap around
71 if ( a > a1 ) { // a is above a1
72 sum_sq += vnl_math::sqr( std::min( a - a1, vnl_math::twopi + a0 - a ) );
73 }
74 else if ( a0 > a ) { // a is below a0
75 sum_sq += vnl_math::sqr( std::min( a0 - a, vnl_math::twopi + a - a1 ) );
76 }
77 }
78 }
79
80 return sum_sq;
81 }
82
83 double
rsdl_dist(const rsdl_point & p,const rsdl_bounding_box & b)84 rsdl_dist( const rsdl_point & p, const rsdl_bounding_box& b )
85 {
86 return std::sqrt( rsdl_dist_sq( p, b ) );
87 }
88
89
90 bool
rsdl_dist_point_in_box(const rsdl_point & pt,const rsdl_bounding_box & box)91 rsdl_dist_point_in_box( const rsdl_point & pt,
92 const rsdl_bounding_box & box )
93 {
94 unsigned int Nc = pt.num_cartesian();
95 unsigned int Na = pt.num_angular();
96 assert( Nc == box.num_cartesian() && Na == box.num_angular() );
97
98 for ( unsigned int i=0; i<Nc; ++i ) {
99 double x = pt.cartesian(i);
100 if ( x < box.min_cartesian(i) || box.max_cartesian(i) < x )
101 return false;
102 }
103
104 for ( unsigned int j=0; j<Na; ++j ) {
105 double a = pt.angular(j);
106 if ( box.min_angular(j) < box.max_angular(j) ) {
107 if ( a < box.min_angular(j) || box.max_angular(j) < a )
108 return false;
109 }
110 else {
111 if ( a > box.max_angular(j) && a < box.min_angular(j) )
112 return false;
113 }
114 }
115
116 return true;
117 }
118
119 void
rsdl_dist_box_relation(const rsdl_bounding_box & inner,const rsdl_bounding_box & outer,bool & inside,bool & intersects)120 rsdl_dist_box_relation( const rsdl_bounding_box & inner,
121 const rsdl_bounding_box & outer,
122 bool& inside,
123 bool& intersects )
124 {
125 unsigned int Nc = inner.num_cartesian();
126 unsigned int Na = inner.num_angular();
127 assert( Nc == outer.num_cartesian() && Na == outer.num_angular() );
128 inside = intersects = true;
129
130 for ( unsigned int i=0; i<Nc && intersects; ++i ) {
131 double x0 = inner.min_cartesian(i), x1 = inner.max_cartesian(i);
132 double y0 = outer.min_cartesian(i), y1 = outer.max_cartesian(i);
133
134 if ( x0 < y0 || y1 < x1 )
135 inside = false;
136
137 if ( y1 < x0 || x1 < y0 )
138 intersects = false;
139 }
140
141 for ( unsigned int j=0; j<Na && intersects; ++j ) {
142 double r0 = inner.min_angular(j), r1 = inner.max_angular(j);
143 double s0 = outer.min_angular(j), s1 = outer.max_angular(j);
144
145 if ( s0 <= s1 ) { // outer angular interval doesn't wrap
146 if ( r0 <= r1 ) { // inner angular interval doesn't wrap
147 if ( r0 < s0 || s1 < r1 ) // part of inner interval is outside
148 inside = false;
149 if ( r1 < s0 || r0 > s1 ) // all of inner interval is to the left or to the right
150 intersects = false;
151 }
152 else { // inner angular interval does wrap
153 inside = false; // point of wrap-around is in inner but not in outer
154 if ( r1 < s0 && s1 < r0 )
155 intersects = false;
156 }
157 }
158 else { // outer angular interval does wrap
159 if ( r0 <= r1 ) { // inner angular interval doesn't wrap
160 if ( r1 > s1 && r0 < s0 )
161 inside = false;
162 if ( r0 > s1 && r1 < s0 )
163 intersects = false;
164 }
165 else { // inner angular interval does wrap; intersects must be true
166 if ( r1 > s1 || r0 < s0 )
167 inside = false;
168 }
169 }
170 }
171 }
172