ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
boundingvolumes.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 #include <adore/mad/adoremath.h>
17 #include <vector>
18 #include <boost/geometry.hpp>
19 #include <boost/geometry/geometries/box.hpp>
20 
21 namespace adore
22 {
23  namespace mad
24  {
25 
26  namespace BoundingVolumes
27  {
28 
29 
36  inline bool hasSeparation_inProjection(float* va,int Na,float* vb,int Nb,float* axis)
37  {
38  float vnew;
39  float amin = adore::mad::dot<3>(axis,va);
40  float amax = amin;
41  float bmin = adore::mad::dot<3>(axis,vb);
42  float bmax = bmin;
43  for(int i=1;i<std::max(Na,Nb);i++)
44  {
45  if(i<Na)
46  {
47  vnew = adore::mad::dot<3>(axis,&va[i*3]);
48  if( vnew>0.0f )
49  {
50  amax+=vnew;
51  }
52  else
53  {
54  amin+=vnew;
55  }
56  }
57  if(i<Nb)
58  {
59  vnew = adore::mad::dot<3>(axis,&vb[i*3]);
60  if( vnew>0.0f )
61  {
62  bmax+=vnew;
63  }
64  else
65  {
66  bmin+=vnew;
67  }
68  }
69  if(!(amax<bmin || bmax<amin))return false;
70  }
71  return (amax<bmin || bmax<amin);
72  }
76  inline bool hasSeparation_testNormals(float* va,int Na,float* vb,int Nb,float* e,int Ne)
77  {
78  float n[3];
79  for(int i=0;i<Ne-1;i++)
80  {
81  for(int j=i+1;j<Ne;j++)
82  {
83  adore::mad::cross(&e[i*3],&e[j*3],n);
84  if(hasSeparation_inProjection(va,Na,vb,Nb,n))return true;
85  }
86  }
87  return false;
88  }
92  inline bool hasSeparation_testNormals(float* va,int Na,float* vb,int Nb,float* ea,int Nea,float* eb,int Neb)
93  {
94  static const float SMALL = 1e-10;
95  float n[3];
96  for(int i=0;i<Nea;i++)
97  {
98  for(int j=0;j<Neb;j++)
99  {
100  adore::mad::cross(&ea[i*3],&eb[j*3],n);
101  if( std::abs(n[0])>SMALL || std::abs(n[1])>SMALL || std::abs(n[2])>SMALL )
102  {
103  if(hasSeparation_inProjection(va,Na,vb,Nb,n))return true;
104  }
105  }
106  }
107  return false;
108  }
109 
113  class OBB3d
114  {
115  private:
116  float v[12];
117  float v_backup[12];
118  public:
119  typedef boost::geometry::model::point<float,3,boost::geometry::cs::cartesian> boost_point;
120  typedef boost::geometry::model::box<boost_point> boost_box;
121 
122  OBB3d(){}
126  OBB3d(OBB3d* other)
127  {
128  std::memcpy(this->v_backup,other->v,12*sizeof(float));
130  }
134  OBB3d(float* values)
135  {
136  std::memcpy(this->v_backup,values,12*sizeof(float));
138  }
142  void setData(float* values)
143  {
144  std::memcpy(this->v_backup,values,12*sizeof(float));
146  }
151  {
152  std::memcpy(this->v,this->v_backup,12*sizeof(float));
153  }
157  void transform_forwards(float cos_psi,float sin_psi,float dx,float dy,float dz)
158  {
159  for(int i=0;i<4;i++)
160  {
161  v[i*3+0] = cos_psi * v_backup[i*3+0] - sin_psi * v_backup[i*3+1];
162  v[i*3+1] = sin_psi * v_backup[i*3+0] + cos_psi * v_backup[i*3+1];
163  v[i*3+2] = v_backup[i*3+2];
164  }
165  v[0]+=dx;
166  v[1]+=dy;
167  v[2]+=dz;
168  }
174  bool isCollisionFree(OBB3d* other)
175  {
176  if(hasSeparation_testNormals(this->v,4,other->v,4,&this->v[3], 3))return true; //face normals of this: 3*2*4 projections
177  if(hasSeparation_testNormals(this->v,4,other->v,4,&other->v[3], 3))return true; //face normals of other: 3*2*4 projections
178  if(hasSeparation_testNormals(this->v,4,other->v,4,&this->v[3], 3,&other->v[3], 3))return true; //normals of edge to edge combinations: 3*3*2*4 projections
179  return false;
180  }
185  bool isPointInside(float x,float y,float z)
186  {
187  float p[3];
188  p[0]=x-v[3*0+0];
189  p[1]=y-v[3*0+1];
190  p[2]=z-v[3*0+2];
191  float d = adore::mad::dot<3>(&v[1*3+0],p);//continue here
192  if(d<0.0f ||d>1.0f)return false;
193  d = adore::mad::dot<3>(&v[2*3+0],p);//continue here
194  if(d<0.0f ||d>1.0f)return false;
195  d = adore::mad::dot<3>(&v[3*3+0],p);//continue here
196  if(d<0.0f ||d>1.0f)return false;
197  return true;
198  }
202  void rotateZ(float cos,float sin,int istart=0)
203  {
204  float tmp;
205  for(int i=istart;i<4;i++)
206  {
207  tmp = cos*v[i*3+0]-sin*v[i*3+1];
208  v[i*3+1] = sin*v[i*3+0]-cos*v[i*3+1];
209  v[i*3+0] = tmp;
210  }
211  }
215  void translate(float* t)
216  {
217  for(int i=0;i<3;i++)v[i]+=t[i];
218  }
219  float* getData(){return v;}
223  void getPoint(float d1,float d2,float d3,float* value)
224  {
225  for(int i=0;i<3;i++)
226  {
227  value[i] = v[i] + d1*v[1*3+i] + d2*v[2*3+i] + d3*v[3*3+i];
228  }
229  }
234  {
235  float min[3];
236  float max[3];
237  float val;
238 
239  for( int i=0;i<4;i++ )
240  {
241  for(int d=0;d<3;d++)
242  {
243  if(i==0)
244  {
245  min[d] = v[d];
246  max[d] = v[d];
247  }
248  else
249  {
250  val = v[i*3+d];
251  if(val>0.0f)
252  {
253  max[d]+=val;
254  }
255  else
256  {
257  min[d]+=val;
258  }
259  }
260  }
261  }
262  return boost_box( boost_point(min[0],min[1],min[2]),
263  boost_point(max[0],max[1],max[2]));
264  }
265 
269  void set_obb(float cx,float cy,float psi,float ac,float bd,float w,float zmin,float zmax)
270  {
271  float w2 = w * 0.5;
272  float c = std::cos(psi);
273  float s = std::sin(psi);
274 
275  //lower rear right corner point
276  v_backup[0*3+0] = cx + c * (-bd) - s * (-w2);
277  v_backup[0*3+1] = cy + s * (-bd) + c * (-w2);
278  v_backup[0*3+2] = zmin;
279  //e0 - forward
280  v_backup[1*3+0] = c*(ac+bd);
281  v_backup[1*3+1] = s*(ac+bd);
282  v_backup[1*3+2] = 0.0f;
283  //e1 - left
284  v_backup[2*3+0] = -s*w;
285  v_backup[2*3+1] = +c*w;
286  v_backup[2*3+2] = 0.0f;
287  //e2 - up
288  v_backup[3*3+0] = 0.0f;
289  v_backup[3*3+1] = 0.0f;
290  v_backup[3*3+2] = zmax-zmin;
291 
293  }
294 
299  void bound_points2d(float* pL,float* pR,float zmin,float zmax,int count)
300  {
301  float dx = (pL[(count-1)*2+0]+pR[(count-1)*2+0])*0.5f - (pL[0*2+0]+pR[0*2+0])*0.5f;
302  float dy = (pL[(count-1)*2+1]+pR[(count-1)*2+1])*0.5f - (pL[0*2+1]+pR[0*2+1])*0.5f;
303  float L = 1.0f/(std::sqrt)(dx*dx+dy*dy);
304  //e1
305  v_backup[1*3+0] = dx*L;
306  v_backup[1*3+1] = dy*L;
307  v_backup[1*3+2] = 0.0f;
308  //e2
309  v_backup[2*3+0] = -dx*L;
310  v_backup[2*3+1] = dx*L;
311  v_backup[2*3+2] = 0.0f;
312  //e3
313  v_backup[3*3+0] = 0.0f;
314  v_backup[3*3+1] = 0.0f;
315  v_backup[3*3+2] = (zmax-zmin);
316  //dimensions
317  float min1 = adore::mad::dot<2>(&v_backup[1*3+0],pL);
318  float max1 = min1;
319  float min2 = adore::mad::dot<2>(&v_backup[2*3+0],pL);
320  float max2 = min2;
321  float p1,p2;
322  for(int i=0;i<count;i++)
323  {
324  p1 = adore::mad::dot<2>(&v_backup[1*3+0],&pL[2*i+0]);
325  p2 = adore::mad::dot<2>(&v_backup[2*3+0],&pL[2*i+0]);
326  min1 = (std::min)(min1,p1);
327  min2 = (std::min)(min2,p2);
328  max1 = (std::min)(max1,p1);
329  max2 = (std::min)(max2,p2);
330  p1 = adore::mad::dot<2>(&v_backup[1*3+0],&pR[2*i+0]);
331  p2 = adore::mad::dot<2>(&v_backup[2*3+0],&pR[2*i+0]);
332  min1 = (std::min)(min1,p1);
333  min2 = (std::min)(min2,p2);
334  max1 = (std::min)(max1,p1);
335  max2 = (std::min)(max2,p2);
336  }
337  //c
338  v_backup[0*3+0] = v_backup[1*3+0]*min1 + v_backup[2*3+0]*min2;
339  v_backup[0*3+1] = v_backup[1*3+1]*min1 + v_backup[2*3+1]*min2;
340  v_backup[0*3+2] = zmin;
341  //e1
342  v_backup[1*3+0]*= (max1-min1);
343  v_backup[1*3+1]*= (max1-min1);
344  //e2
345  v_backup[2*3+0]*= (max2-min2);
346  v_backup[2*3+1]*= (max2-min2);
347 
349  }
350 
351 
355  void mean(float* x,int n,float& xm)
356  {
357  xm = 0.0f;
358  for(int i=0;i<n;i++)xm+=x[i];
359  xm/=(float)n;
360  }
364  void projected_interval(float* x,float* y,int n,float bx,float by,float cx,float cy,float& bxmin,float& bxmax)
365  {
366  bxmin = (x[0]-cx)*bx+(y[0]-cy)*by;
367  bxmax = bxmin;
368  for(int i=1;i<n;i++)
369  {
370  float val = (x[i]-cx)*bx+(y[i]-cy)*by;
371  if(val<bxmin)
372  {
373  bxmin=val;
374  }
375  else if(val>bxmax)
376  {
377  bxmax=val;
378  }
379  }
380  }
381 
387  void bound_points3d(int n0,int n1,float* x0,float* x1,float* y0,float* y1,float z0,float z1)
388  {
389  float xm0,ym0,xm1,ym1;
390  mean(x0,n0,xm0);
391  mean(x1,n1,xm1);
392  mean(y0,n0,ym0);
393  mean(y1,n1,ym1);
394  //e2 -- the center(S1) to center(S2) vector
395  v_backup[3*3+0] = xm1-xm0;
396  v_backup[3*3+1] = ym1-ym0;
397  v_backup[3*3+2] = z1-z0;
398  float bx,by,bl;//base vector x,y, projected length
399  bl = std::sqrt(v_backup[3*3+0]*v_backup[3*3+0]+v_backup[3*3+1]*v_backup[3*3+1]);
400  if( bl>0.1 )
401  {
402  bx = v_backup[3*3+0]/bl;
403  by = v_backup[3*3+1]/bl;
404  }
405  else
406  {
407  if( n0>1 )
408  {
409  bx = x0[1]-x0[0];
410  by = y0[1]-y0[0];
411  bl = std::sqrt(bx*bx+by*by);
412  if( bl>1e-5 )
413  {
414  bx = bx / bl;
415  by = by / bl;
416  }
417  else
418  {
419  bx = 1.0;
420  by = 0.0;
421  }
422  }
423  else
424  {
425  bx = 1.0;
426  by = 0.0;
427  }
428  }
429  float e00min,e00max,e10min,e10max;//interval for first box
430  float e01min,e01max,e11min,e11max;//interval for second box
431  projected_interval(x0,y0,n0,bx,by,xm0,ym0,e00min,e00max);//interval for e0 direction for box0
432  projected_interval(x0,y0,n0,-by,bx,xm0,ym0,e10min,e10max);//interval for e1 direction for box0
433  projected_interval(x1,y1,n1,bx,by,xm1,ym1,e01min,e01max);//interval for e0 direction for box1
434  projected_interval(x1,y1,n1,-by,bx,xm1,ym1,e11min,e11max);//interval for e1 direction for box1
435  //set e0 -- extension of box along e2 projection
436  v_backup[1*3+0] = bx * std::max(e00max-e00min,e01max-e01min);
437  v_backup[1*3+1] = by * std::max(e00max-e00min,e01max-e01min);
438  v_backup[1*3+2] = 0.0;
439  //set e1 -- extension of box along e2 projection normal (left)
440  v_backup[2*3+0] = -by * std::max(e10max-e10min,e11max-e11min);
441  v_backup[2*3+1] = +bx * std::max(e10max-e10min,e11max-e11min);
442  v_backup[2*3+2] = 0.0;
443  //set c -- lower right rear corner point
444  v_backup[0*3+0] = xm0 + bx*std::min(e00min,e01min) - by*std::min(e10min,e11min);
445  v_backup[0*3+1] = ym0 + by*std::min(e00min,e01min) + bx*std::min(e10min,e11min);
446  v_backup[0*3+2] = z0;
447 
449  }
450  };
451 
457  class OBBTree3d
458  {
459  private:
464  bool child_valid;
465  bool box_valid;
466  void initSibling(OBBTree3d* other)
467  {
468  if(sibling==0)sibling = new OBBTree3d();
469  sibling->copy(other);
470  }
471  void initChild(OBBTree3d* other)
472  {
473  if(child==0)child = new OBBTree3d();
474  child->copy(other);
475  }
476  void initBox(float* values)
477  {
478  if(box==0)box = new OBB3d();
479  box->setData(values);
480  }
481  public:
486  {
487  sibling=0;
488  child=0;
489  box=0;
490  sibling_valid=false;
491  child_valid=false;
492  box_valid=false;
493  }
498  {
499  if(box!=0)delete box;
500  if(sibling!=0)delete sibling;
501  if(child!=0)delete child;
502  }
506  void setBox(OBB3d* box)
507  {
508  this->box = box;
509  box_valid = (box!=0);
510  }
515  {
516  this->child = child;
517  child_valid = (child!=0);
518  }
523  {
524  this->sibling = sibling;
525  sibling_valid = (sibling!=0);
526  }
530  void copy(OBBTree3d* other)
531  {
532  if(sibling_valid=other->sibling_valid)initSibling(other->sibling); //the single "=" is correct
533  if(child_valid=other->child_valid)initChild(other->child); //the single "=" is correct
534  if(box_valid=other->box_valid)initBox(other->box->getData()); //the single "=" is correct
535  }
539  void rotateZ(float cos,float sin,int istart=0)
540  {
541  if(box_valid)box->rotateZ(cos,sin,istart);
542  if(sibling_valid)sibling->rotateZ(cos,sin,istart);
543  if(child_valid)child->rotateZ(cos,sin,istart);
544  }
548  void translate(float* t)
549  {
550  if(box_valid)box->translate(t);
552  if(child_valid)child->translate(t);
553  }
558  {
559  if(sibling_valid)return sibling;
560  else return 0;
561  }
566  {
567  if(child_valid)return child;
568  else return 0;
569  }
574  {
575  if(box_valid)return box;
576  else return 0;
577  }
581  void transform_forwards(float cos_psi,float sin_psi,float dx,float dy,float dz)
582  {
583  if(this->box_valid)
584  {
585  this->box->transform_forwards(cos_psi,sin_psi,dx,dy,dz);
586  }
587  if(this->getChild()!=0)this->getChild()->transform_forwards(cos_psi,sin_psi,dx,dy,dz);
588  if(this->getSibling()!=0)this->getSibling()->transform_forwards(cos_psi,sin_psi,dx,dy,dz);
589  }
590 
595  bool isCollisionFree(OBBTree3d* other_root)
596  {
597  if( this->box_valid )
598  {
599  for(OBBTree3d* other = other_root;other!=nullptr;other = other->getSibling())
600  {
601  if(other->box_valid)
602  {
603  if( this->box->isCollisionFree(other->box) )
604  {
605  continue;
606  }
607  else
608  {
609  if(!this->child_valid && !other->child_valid)
610  {
611  return false;
612  }
613  else
614  {
615  OBBTree3d* A = this->child_valid?this->child:this;
616  OBBTree3d* B = other->child_valid?other->child:other;
617  if( A->isCollisionFree(B) )
618  {
619  continue;
620  }
621  else
622  {
623  return false;
624  }
625  }
626  }
627  }
628  }
629  }
630  else
631  {
632  if( this->child_valid )
633  {
634  if( !this->child->isCollisionFree( other_root ) )
635  {
636  return false;
637  }
638  }
639  }
640  if( this->sibling_valid )
641  {
642  return this->sibling->isCollisionFree(other_root);
643  }
644  else
645  {
646  return true;
647  }
648  /*if(this->box_valid && other->box_valid && this->box->isCollisionFree(other->box))
649  {
650  return true;
651  }
652  else
653  {
654  if( this->getChild()==0 && other->getChild()==0 )
655  {
656  return false;
657  }
658  else
659  {
660  if(this->getChild()!=0 && other->getChild()!=0)
661  {
662  for(OBBTree3d* tree0 = this->getChild(); tree0!=0; tree0=tree0->getSibling())
663  {
664  for(OBBTree3d* tree1 = other->getChild(); tree1!=0; tree1=tree1->getSibling())
665  {
666  if(!tree0->isCollisionFree(tree1))return false;
667  }
668  }
669  return true;
670  }
671  else
672  {
673  OBBTree3d* noChildTree = (this->getChild()==0)?this:other;
674  OBBTree3d* hasChildTree = (this->getChild()==0)?other:this;
675  if( !noChildTree->box_valid )return false;
676  for( OBBTree3d* tree0 = hasChildTree->getChild(); tree0!=0; tree0=tree0->getSibling() )
677  {
678  if( !tree0->isCollisionFree( noChildTree->getBox() ) )return false;
679  }
680  return true;
681  }
682  }
683  }*/
684  }
685 
689  bool isCollisionFree(OBB3d* other)
690  {
691  if(this->box_valid && (other==0 || this->box->isCollisionFree(other)))
692  {
693  return true;
694  }
695  else
696  {
697  if( this->getChild()==0 )
698  {
699  return false;
700  }
701  else
702  {
703  for(OBBTree3d* tree0 = this->getChild(); tree0!=0; tree0=tree0->getSibling())
704  {
705  if(!tree0->isCollisionFree(other))return false;
706  }
707  return true;
708  }
709  }
710  }
711 
712  };
713 
717  template<int Nmax>
719  {
720  private:
721  //float* v;//[ c,e0,e1, c,e0,e1, c,e0,e1]
722  float v[Nmax*3*3];
723  int N;
726  {
727  float* obbv = other->getData();
728  for(int i=0;i<N;i++)
729  {
730  if(hasSeparation_testNormals(&this->v[3*3*i],3,obbv,4,&this->v[3*3*i+3],2))continue; //test normal of this plane
731  if(hasSeparation_testNormals(&this->v[3*3*i],3,obbv,4,&obbv[3],3))continue; //test normals of obb
732  if(hasSeparation_testNormals(&this->v[3*3*i],3,obbv,4,&this->v[3*3*i+3],2,&obbv[3],3))continue; //test edge combinations ei x ej
733  return false;
734  }
735  return true;
736  }
737  void generatePlaneData(float* p,float* n)
738  {
739  for(int i=0;i<N;i++)
740  {
741  //c
742  v[i*3*3+0+0]=p[i*3+0];
743  v[i*3*3+0+1]=p[i*3+1];
744  v[i*3*3+0+2]=p[i*3+2];
745 
746  //e0
747  v[i*3*3+3+0]=p[(i+1)*3+0]-p[i*3+0];
748  v[i*3*3+3+1]=p[(i+1)*3+1]-p[i*3+1];
749  v[i*3*3+3+2]=p[(i+1)*3+2]-p[i*3+2];
750 
751  //e1
752  v[i*3*3+6+0]=n[i*3+0];
753  v[i*3*3+6+1]=n[i*3+1];
754  v[i*3*3+6+2]=n[i*3+2];
755  }
756  }
757  void generateBoundingVolume(float* p,float* n)
758  {
759  float* bv = box.getData();
760  bv[1*3+0] = p[N*3+0] - p[0*3+0]; //first box axis is first to last point
761  bv[1*3+1] = p[N*3+1] - p[0*3+1];
762  bv[1*3+2] = p[N*3+2] - p[0*3+2];
763  adore::mad::normalize<3>(&bv[1*3]);
764 
765  //if the first axis and n are not perpenticular, compute a vector which is perpendicular to first axis and pointing in similar direction as n
766  float h[3];
767  adore::mad::cross(&bv[1*3],n,h);
768  adore::mad::cross(&bv[1*3],h,&bv[2*3]); //second box axis, perpendicular to first, similar to n
769  adore::mad::normalize<3>(&bv[2*3]);
770 
771  adore::mad::cross(&bv[1*3],&bv[2*3],&bv[3*3]); //third box axis is normal to the first two
772  adore::mad::normalize<3>(&bv[3*3]);
773 
774  float min[3];
775  float max[3];
776  min[0] = adore::mad::dot<3>(&bv[1*3],p); //initialize bounds with first point
777  min[1] = adore::mad::dot<3>(&bv[2*3],p); //initialize bounds with first point
778  min[2] = adore::mad::dot<3>(&bv[3*3],p); //initialize bounds with first point
779  max[0]=min[0];
780  max[1]=min[1];
781  max[2]=min[2];
782 
783 
784  for(int i=1;i<=N;i++)
785  {
786  for(int j=1;j<=3;j++)
787  {
788  float pp = adore::mad::dot<3>(&bv[j*3],&p[i*3]);
789  float pn = adore::mad::dot<3>(&bv[j*3],&n[i*3]);
790  min[j-1] = std::min(min[j-1],pp + (pn<0.0f?pn:0.0f));
791  max[j-1] = std::max(max[j-1],pp + (pn>0.0f?pn:0.0f));
792  }
793  }
794  //set the corner point
795  bv[0] = min[0]*bv[3*1+0] + min[1]*bv[3*2+0] + min[2]*bv[3*3+0];
796  bv[1] = min[0]*bv[3*1+1] + min[1]*bv[3*2+1] + min[2]*bv[3*3+1];
797  bv[2] = min[0]*bv[3*1+2] + min[1]*bv[3*2+2] + min[2]*bv[3*3+2];
798  //scale the edges
799  for(int i=1;i<=3;i++)
800  {
801  for(int j=0;j<3;j++)
802  {
803  bv[i*3+j]*=(max[i-1]-min[i-1]);
804  }
805  }
806  }
807  public:
813  {
814  N=0;
815  }
819  PlaneSequence3d(float* p,float* n,int k)
820  {
821  initialize(p,n,k);
822  }
823  void initialize(float* p,float* n,int k)
824  {
825  N = k-1;
826  assert(N<=Nmax);
827  //v = new float[N*3*3];
828  generatePlaneData(p,n);
830  }
832  {
833  //delete[] v;
834  }
839  bool isCollisionFree(OBB3d* other)
840  {
841  if( box.isCollisionFree(other) )
842  {
843  return true;
844  }
845  else
846  {
847  return isCollisionFree_detailedTest(other);
848  }
849  }
854  {
855  //iterate on the current level of the tree
856  for(OBBTree3d* tree = other;tree!=0;tree = tree->getSibling())
857  {
858  //if tree gives no box or if the box is in collision with this box: investigate
859  if(tree->getBox()==0 || !this->box.isCollisionFree(tree->getBox()))
860  {
861  if(tree->getChild()==0)
862  {
863  //if the tree has no children (is a node), execute detailed test
864  if(tree->getBox()!=0 && !isCollisionFree_detailedTest(tree->getBox()))return false;
865  }
866  else
867  {
868  //otherwise postpone evaluation
869  if(!this->isCollisionFree(tree->getChild()))return false;
870  }
871  }
872  }
873  return true;
874  }
875  OBB3d* getOBB(){return &box;}
876  float* getData(){return v;}
877  int size(){return N;}
878  };
879 
880 
881 
882  }
883  }
884 }
Definition: boundingvolumes.h:114
bool isPointInside(float x, float y, float z)
Definition: boundingvolumes.h:185
OBB3d(OBB3d *other)
Definition: boundingvolumes.h:126
float v[12]
Definition: boundingvolumes.h:116
float * getData()
Definition: boundingvolumes.h:219
OBB3d()
Definition: boundingvolumes.h:122
void set_obb(float cx, float cy, float psi, float ac, float bd, float w, float zmin, float zmax)
Definition: boundingvolumes.h:269
void projected_interval(float *x, float *y, int n, float bx, float by, float cx, float cy, float &bxmin, float &bxmax)
Definition: boundingvolumes.h:364
void bound_points3d(int n0, int n1, float *x0, float *x1, float *y0, float *y1, float z0, float z1)
Definition: boundingvolumes.h:387
boost::geometry::model::point< float, 3, boost::geometry::cs::cartesian > boost_point
Definition: boundingvolumes.h:119
bool isCollisionFree(OBB3d *other)
Definition: boundingvolumes.h:174
void rotateZ(float cos, float sin, int istart=0)
Definition: boundingvolumes.h:202
boost_box getAABox()
Definition: boundingvolumes.h:233
float v_backup[12]
Definition: boundingvolumes.h:117
void getPoint(float d1, float d2, float d3, float *value)
Definition: boundingvolumes.h:223
void setData(float *values)
Definition: boundingvolumes.h:142
void resetTransformation()
Definition: boundingvolumes.h:150
OBB3d(float *values)
Definition: boundingvolumes.h:134
boost::geometry::model::box< boost_point > boost_box
Definition: boundingvolumes.h:120
void transform_forwards(float cos_psi, float sin_psi, float dx, float dy, float dz)
Definition: boundingvolumes.h:157
void bound_points2d(float *pL, float *pR, float zmin, float zmax, int count)
Definition: boundingvolumes.h:299
void translate(float *t)
Definition: boundingvolumes.h:215
void mean(float *x, int n, float &xm)
Definition: boundingvolumes.h:355
Definition: boundingvolumes.h:458
void transform_forwards(float cos_psi, float sin_psi, float dx, float dy, float dz)
Definition: boundingvolumes.h:581
void setChild(OBBTree3d *child)
Definition: boundingvolumes.h:514
void copy(OBBTree3d *other)
Definition: boundingvolumes.h:530
void initChild(OBBTree3d *other)
Definition: boundingvolumes.h:471
bool isCollisionFree(OBBTree3d *other_root)
Definition: boundingvolumes.h:595
OBB3d * box
Definition: boundingvolumes.h:462
OBBTree3d * sibling
Definition: boundingvolumes.h:460
OBBTree3d * getSibling()
Definition: boundingvolumes.h:557
bool sibling_valid
Definition: boundingvolumes.h:463
void rotateZ(float cos, float sin, int istart=0)
Definition: boundingvolumes.h:539
OBBTree3d * child
Definition: boundingvolumes.h:461
void setSibling(OBBTree3d *sibling)
Definition: boundingvolumes.h:522
void translate(float *t)
Definition: boundingvolumes.h:548
OBB3d * getBox()
Definition: boundingvolumes.h:573
OBBTree3d * getChild()
Definition: boundingvolumes.h:565
bool child_valid
Definition: boundingvolumes.h:464
bool box_valid
Definition: boundingvolumes.h:465
~OBBTree3d()
Definition: boundingvolumes.h:497
void setBox(OBB3d *box)
Definition: boundingvolumes.h:506
bool isCollisionFree(OBB3d *other)
Definition: boundingvolumes.h:689
void initSibling(OBBTree3d *other)
Definition: boundingvolumes.h:466
void initBox(float *values)
Definition: boundingvolumes.h:476
OBBTree3d()
Definition: boundingvolumes.h:485
Definition: boundingvolumes.h:719
bool isCollisionFree(OBBTree3d *other)
Definition: boundingvolumes.h:853
PlaneSequence3d()
Definition: boundingvolumes.h:812
void generatePlaneData(float *p, float *n)
Definition: boundingvolumes.h:737
OBB3d box
Definition: boundingvolumes.h:724
bool isCollisionFree_detailedTest(OBB3d *other)
Definition: boundingvolumes.h:725
int size()
Definition: boundingvolumes.h:877
bool isCollisionFree(OBB3d *other)
Definition: boundingvolumes.h:839
OBB3d * getOBB()
Definition: boundingvolumes.h:875
~PlaneSequence3d()
Definition: boundingvolumes.h:831
float * getData()
Definition: boundingvolumes.h:876
void initialize(float *p, float *n, int k)
Definition: boundingvolumes.h:823
int N
Definition: boundingvolumes.h:723
float v[Nmax *3 *3]
Definition: boundingvolumes.h:722
PlaneSequence3d(float *p, float *n, int k)
Definition: boundingvolumes.h:819
void generateBoundingVolume(float *p, float *n)
Definition: boundingvolumes.h:757
bool hasSeparation_inProjection(float *va, int Na, float *vb, int Nb, float *axis)
Definition: boundingvolumes.h:36
bool hasSeparation_testNormals(float *va, int Na, float *vb, int Nb, float *e, int Ne)
Definition: boundingvolumes.h:76
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
T * cross(T *u, T *v, T *s)
Definition: adoremath.h:599
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x0
Definition: adore_set_goal.py:25
x
Definition: adore_set_goal.py:30
y0
Definition: adore_set_goal.py:26
z0
Definition: adore_set_goal.py:27
y
Definition: adore_set_goal.py:31
z
Definition: adore_set_goal.py:32
y1
Definition: adore_set_pose.py:29
x1
Definition: adore_set_pose.py:28
z1
Definition: adore_set_pose.py:30
w
Definition: adore_set_pose.py:40
Definition: areaofeffectconverter.h:20