18 #include <boost/geometry.hpp>
19 #include <boost/geometry/geometries/box.hpp>
26 namespace BoundingVolumes
39 float amin = adore::mad::dot<3>(axis,va);
41 float bmin = adore::mad::dot<3>(axis,vb);
47 vnew = adore::mad::dot<3>(axis,&va[i*3]);
59 vnew = adore::mad::dot<3>(axis,&vb[i*3]);
69 if(!(amax<bmin || bmax<amin))
return false;
71 return (amax<bmin || bmax<amin);
79 for(
int i=0;i<Ne-1;i++)
81 for(
int j=i+1;j<Ne;j++)
94 static const float SMALL = 1e-10;
96 for(
int i=0;i<Nea;i++)
98 for(
int j=0;j<Neb;j++)
101 if( std::abs(n[0])>SMALL || std::abs(n[1])>SMALL || std::abs(n[2])>SMALL )
119 typedef boost::geometry::model::point<float,3,boost::geometry::cs::cartesian>
boost_point;
120 typedef boost::geometry::model::box<boost_point>
boost_box;
128 std::memcpy(this->v_backup,other->
v,12*
sizeof(
float));
136 std::memcpy(this->v_backup,values,12*
sizeof(
float));
144 std::memcpy(this->v_backup,values,12*
sizeof(
float));
152 std::memcpy(this->v,this->v_backup,12*
sizeof(
float));
191 float d = adore::mad::dot<3>(&
v[1*3+0],p);
192 if(d<0.0f ||d>1.0f)
return false;
193 d = adore::mad::dot<3>(&
v[2*3+0],p);
194 if(d<0.0f ||d>1.0f)
return false;
195 d = adore::mad::dot<3>(&
v[3*3+0],p);
196 if(d<0.0f ||d>1.0f)
return false;
205 for(
int i=istart;i<4;i++)
217 for(
int i=0;i<3;i++)
v[i]+=t[i];
223 void getPoint(
float d1,
float d2,
float d3,
float* value)
227 value[i] =
v[i] + d1*
v[1*3+i] + d2*
v[2*3+i] + d3*
v[3*3+i];
239 for(
int i=0;i<4;i++ )
269 void set_obb(
float cx,
float cy,
float psi,
float ac,
float bd,
float w,
float zmin,
float zmax)
276 v_backup[0*3+0] = cx + c * (-bd) - s * (-w2);
277 v_backup[0*3+1] = cy + s * (-bd) + c * (-w2);
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);
317 float min1 = adore::mad::dot<2>(&
v_backup[1*3+0],pL);
319 float min2 = adore::mad::dot<2>(&
v_backup[2*3+0],pL);
322 for(
int i=0;i<count;i++)
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]);
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]);
358 for(
int i=0;i<n;i++)xm+=
x[i];
366 bxmin = (
x[0]-cx)*bx+(
y[0]-cy)*by;
370 float val = (
x[i]-cx)*bx+(
y[i]-cy)*by;
389 float xm0,ym0,xm1,ym1;
411 bl = std::sqrt(bx*bx+by*by);
429 float e00min,e00max,e10min,e10max;
430 float e01min,e01max,e11min,e11max;
597 if( this->box_valid )
609 if(!this->child_valid && !other->child_valid)
632 if( this->child_valid )
640 if( this->sibling_valid )
705 if(!tree0->isCollisionFree(other))
return false;
727 float* obbv = other->
getData();
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];
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];
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];
760 bv[1*3+0] = p[
N*3+0] - p[0*3+0];
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]);
769 adore::mad::normalize<3>(&bv[2*3]);
772 adore::mad::normalize<3>(&bv[3*3]);
776 min[0] = adore::mad::dot<3>(&bv[1*3],p);
777 min[1] = adore::mad::dot<3>(&bv[2*3],p);
778 min[2] = adore::mad::dot<3>(&bv[3*3],p);
784 for(
int i=1;i<=
N;i++)
786 for(
int j=1;j<=3;j++)
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]);
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];
799 for(
int i=1;i<=3;i++)
803 bv[i*3+j]*=(
max[i-1]-
min[i-1]);
859 if(tree->getBox()==0 || !this->box.isCollisionFree(tree->getBox()))
861 if(tree->getChild()==0)
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