38 #include <LinearMath/btConvexHull.h> 64 std::cerr <<
"Creating body from shape: Unknown shape type" << shape->
type << std::endl;
80 mergedSphere = spheres[0];
81 for (
unsigned int i = 1 ; i < spheres.size() ; ++i)
83 if (spheres[i].radius <= 0.0)
85 double d = spheres[i].center.distance(mergedSphere.
center);
86 if (d + mergedSphere.
radius <= spheres[i].radius)
88 mergedSphere.
center = spheres[i].center;
89 mergedSphere.
radius = spheres[i].radius;
92 if (d + spheres[i].radius > mergedSphere.
radius)
104 static const double ZERO = 1e-9;
111 double d = dir.
dot(a);
140 return (m_center - p).length2() < m_radius2;
150 m_radiusU = m_radius * m_scale + m_padding;
151 m_radius2 = m_radiusU * m_radiusU;
152 m_center = m_pose.getOrigin();
157 return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0;
163 sphere.
radius = m_radiusU;
168 if (
distanceSQR(m_center, origin, dir) > m_radius2)
return false;
173 double dpcpv = cp.
dot(dir);
177 double x = m_radius2 - w.length2();
182 double dpQv = w.
dot(dir);
186 intersections->push_back(Q);
197 double dpAv = w.
dot(dir);
199 double dpBv = w.dot(dir);
206 intersections->push_back(A);
216 intersections->push_back(B);
225 double pH = v.
dot(m_normalH);
227 if (fabs(pH) > m_length2)
230 double pB1 = v.
dot(m_normalB1);
231 double remaining = m_radius2 - pB1 * pB1;
237 double pB2 = v.
dot(m_normalB2);
238 return pB2 * pB2 < remaining;
250 m_radiusU = m_radius * m_scale + m_padding;
251 m_radius2 = m_radiusU * m_radiusU;
252 m_length2 = m_scale * m_length / 2.0 + m_padding;
253 m_center = m_pose.getOrigin();
254 m_radiusBSqr = m_length2 * m_length2 + m_radius2;
255 m_radiusB = sqrt(m_radiusBSqr);
262 double tmp = -m_normalH.
dot(m_center);
263 m_d1 = tmp + m_length2;
264 m_d2 = tmp - m_length2;
269 return 2.0 * M_PI * m_radius2 * m_length2;
275 sphere.
radius = m_radiusB;
280 if (
distanceSQR(m_center, origin, dir) > m_radiusBSqr)
return false;
282 std::vector<detail::intersc> ipts;
285 double tmp = m_normalH.dot(dir);
286 if (fabs(tmp) >
ZERO)
288 double tmp2 = -m_normalH.dot(origin);
289 double t1 = (tmp2 - m_d1) / tmp;
295 v1 = v1 - m_normalH.
dot(v1) * m_normalH;
298 if (intersections == NULL)
306 double t2 = (tmp2 - m_d2) / tmp;
311 v2 = v2 - m_normalH.
dot(v2) * m_normalH;
314 if (intersections == NULL)
327 tf::Vector3 ROD(m_normalH.cross(origin - m_center));
329 double b = 2.0 * ROD.dot(VD);
330 double c = ROD.length2() - m_radius2;
331 double d = b * b - 4.0 * a * c;
332 if (d > 0.0 && fabs(a) >
ZERO)
336 double t1 = (b + d) / e;
337 double t2 = (b - d) / e;
344 if (fabs(m_normalH.dot(v1)) < m_length2 +
ZERO)
346 if (intersections == NULL)
359 if (fabs(m_normalH.dot(v2)) < m_length2 +
ZERO)
361 if (intersections == NULL)
374 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
375 for (
unsigned int i = 0 ; i < n ; ++i)
376 intersections->push_back(ipts[i].pt);
386 double pL = v.
dot(m_normalL);
388 if (fabs(pL) > m_length2)
391 double pW = v.
dot(m_normalW);
393 if (fabs(pW) > m_width2)
396 double pH = v.
dot(m_normalH);
398 if (fabs(pH) > m_height2)
406 const double *size =
static_cast<const shapes::Box*
>(shape)->size;
414 double s2 = m_scale / 2.0;
415 m_length2 = m_length * s2 + m_padding;
416 m_width2 = m_width * s2 + m_padding;
417 m_height2 = m_height * s2 + m_padding;
419 m_center = m_pose.getOrigin();
421 m_radius2 = m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2;
422 m_radiusB = sqrt(m_radius2);
429 const tf::Vector3 tmp(m_normalL * m_length2 + m_normalW * m_width2 + m_normalH * m_height2);
430 m_corner1 = m_center - tmp;
431 m_corner2 = m_center + tmp;
436 return 8.0 * m_length2 * m_width2 * m_height2;
442 sphere.
radius = m_radiusB;
447 if (
distanceSQR(m_center, origin, dir) > m_radius2)
return false;
449 double t_near = -INFINITY;
450 double t_far = INFINITY;
452 for (
int i = 0; i < 3; i++)
454 const tf::Vector3 &vN = i == 0 ? m_normalL : (i == 1 ? m_normalW : m_normalH);
455 double dp = vN.
dot(dir);
459 double t1 = vN.
dot(m_corner1 - origin) / dp;
460 double t2 = vN.
dot(m_corner2 - origin) / dp;
481 if ((std::min(m_corner1.y(), m_corner2.y()) > origin.
y() ||
482 std::max(m_corner1.y(), m_corner2.y()) < origin.
y()) &&
483 (std::min(m_corner1.z(), m_corner2.z()) > origin.
z() ||
484 std::max(m_corner1.z(), m_corner2.z()) < origin.
z()))
491 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.
x() ||
492 std::max(m_corner1.x(), m_corner2.x()) < origin.
x()) &&
493 (std::min(m_corner1.z(), m_corner2.z()) > origin.
z() ||
494 std::max(m_corner1.z(), m_corner2.z()) < origin.
z()))
498 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.
x() ||
499 std::max(m_corner1.x(), m_corner2.x()) < origin.
x()) &&
500 (std::min(m_corner1.y(), m_corner2.y()) > origin.
y() ||
501 std::max(m_corner1.y(), m_corner2.y()) < origin.
y()))
509 if (t_far - t_near >
ZERO)
511 intersections->push_back(t_near * dir + origin);
513 intersections->push_back(t_far * dir + origin);
516 intersections->push_back(t_far * dir + origin);
708 if (m_boundingBox.containsPoint(p))
711 ip = m_meshCenter + (ip - m_meshCenter) * m_scale;
712 return isPointInsidePlanes(ip);
722 double maxX = -INFINITY, maxY = -INFINITY, maxZ = -INFINITY;
723 double minX = INFINITY, minY = INFINITY, minZ = INFINITY;
725 for(
unsigned int i = 0; i < mesh->
vertexCount ; ++i)
728 double vy = mesh->
vertices[3 * i + 1];
729 double vz = mesh->
vertices[3 * i + 2];
731 if (maxX < vx) maxX = vx;
732 if (maxY < vy) maxY = vy;
733 if (maxZ < vz) maxZ = vz;
735 if (minX > vx) minX = vx;
736 if (minY > vy) minY = vy;
737 if (minZ > vz) minZ = vz;
740 if (maxX < minX) maxX = minX = 0.0;
741 if (maxY < minY) maxY = minY = 0.0;
742 if (maxZ < minZ) maxZ = minZ = 0.0;
745 m_boundingBox.setDimensions(box_shape);
748 m_boxOffset.setValue((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
756 btVector3 *vertices =
new btVector3[mesh->
vertexCount];
757 for(
unsigned int i = 0; i < mesh->
vertexCount ; ++i)
759 vertices[i].setX(mesh->
vertices[3 * i ]);
760 vertices[i].setY(mesh->
vertices[3 * i + 1]);
761 vertices[i].setZ(mesh->
vertices[3 * i + 2]);
764 HullDesc hd(QF_TRIANGLES, mesh->
vertexCount, vertices);
767 if (hl.CreateConvexHull(hd, hr) == QE_OK)
771 m_vertices.reserve(hr.m_OutputVertices.size());
774 for (
int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
776 tf::Vector3 vector3_tmp(
tf::Vector3(hr.m_OutputVertices[j][0],hr.m_OutputVertices[j][1],hr.m_OutputVertices[j][2]));
777 m_vertices.push_back(vector3_tmp);
778 sum = sum + vector3_tmp;
781 m_meshCenter = sum / (double)(hr.m_OutputVertices.size());
782 for (
unsigned int j = 0 ; j < m_vertices.size() ; ++j)
784 double dist = m_vertices[j].
distance2(m_meshCenter);
785 if (dist > m_meshRadiusB)
786 m_meshRadiusB = dist;
788 m_meshRadiusB = sqrt(m_meshRadiusB);
790 m_triangles.reserve(hr.m_Indices.size());
791 for (
unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
793 const tf::Vector3 p1(hr.m_OutputVertices[hr.m_Indices[j * 3 ]][0],hr.m_OutputVertices[hr.m_Indices[j * 3 ]][1],hr.m_OutputVertices[hr.m_Indices[j * 3 ]][2]);
794 const tf::Vector3 p2(hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]][0],hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]][1],hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]][2]);
795 const tf::Vector3 p3(hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]][0],hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]][1],hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]][2]);
810 unsigned int behindPlane = countVerticesBehindPlane(planeEquation);
813 tf::tfVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW());
814 unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2);
815 if (behindPlane2 < behindPlane)
817 planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW());
818 behindPlane = behindPlane2;
825 m_planes.push_back(planeEquation);
827 m_triangles.push_back(hr.m_Indices[j * 3 + 0]);
828 m_triangles.push_back(hr.m_Indices[j * 3 + 1]);
829 m_triangles.push_back(hr.m_Indices[j * 3 + 2]);
834 std::cerr <<
"Unable to compute convex hull.";
836 hl.ReleaseResult(hr);
845 m_boundingBox.setPose(pose);
846 m_boundingBox.setPadding(m_padding);
847 m_boundingBox.setScale(m_scale);
849 m_iPose = m_pose.inverse();
850 m_center = m_pose * m_meshCenter;
851 m_radiusB = m_meshRadiusB * m_scale + m_padding;
852 m_radiusBSqr = m_radiusB * m_radiusB;
854 m_scaledVertices.resize(m_vertices.size());
855 for (
unsigned int i = 0 ; i < m_vertices.size() ; ++i)
859 m_scaledVertices[i] = m_meshCenter + v * (m_scale + (l >
ZERO ? m_padding / l : 0.0));
866 sphere.
radius = m_radiusB;
871 unsigned int numplanes = m_planes.size();
872 for (
unsigned int i = 0 ; i < numplanes ; ++i)
876 if (dist > tfScalar(0))
884 unsigned int numvertices = m_vertices.size();
885 unsigned int result = 0;
886 for (
unsigned int i = 0 ; i < numvertices ; ++i)
898 for (
unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i)
900 const tf::Vector3 &v1 = m_vertices[m_triangles[3*i + 0]];
901 const tf::Vector3 &v2 = m_vertices[m_triangles[3*i + 1]];
902 const tf::Vector3 &v3 = m_vertices[m_triangles[3*i + 2]];
903 volume += v1.
x()*v2.
y()*v3.
z() + v2.
x()*v3.
y()*v1.
z() + v3.
x()*v1.
y()*v2.
z() - v1.
x()*v3.
y()*v2.
z() - v2.
x()*v1.
y()*v3.
z() - v3.
x()*v2.
y()*v1.
z();
905 return fabs(volume)/6.0;
910 if (
distanceSQR(m_center, origin, dir) > m_radiusBSqr)
return false;
911 if (!m_boundingBox.intersectsRay(origin, dir))
return false;
917 std::vector<detail::intersc> ipts;
922 const unsigned int nt = m_triangles.size() / 3;
923 for (
unsigned int i = 0 ; i < nt ; ++i)
926 if (fabs(tmp) >
ZERO)
928 double t = -(m_planes[i].dot(orig) + m_planes[i].getW()) / tmp;
931 const int i3 = 3 * i;
932 const int v1 = m_triangles[i3 + 0];
933 const int v2 = m_triangles[i3 + 1];
934 const int v3 = m_triangles[i3 + 2];
950 if (c1.dot(c2) < 0.0)
959 if (c1.dot(c2) < 0.0)
965 if (c1.dot(c2) < 0.0)
983 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
984 for (
unsigned int i = 0 ; i < n ; ++i)
985 intersections->push_back(ipts[i].pt);
static double distanceSQR(const tf::Vector3 &p, const tf::Vector3 &origin, const tf::Vector3 &dir)
Compute the square of the distance between a ray and a point Note: this requires 'dir' to be normaliz...
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
virtual void useDimensions(const shapes::Shape *shape)
Definition of a cylinder.
virtual void updateInternalData(void)
unsigned int countVerticesBehindPlane(const tf::tfVector4 &planeNormal) const
Definition of a cylinder.
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual void useDimensions(const shapes::Shape *shape)
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual void updateInternalData(void)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
virtual void updateInternalData(void)
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual void useDimensions(const shapes::Shape *shape)
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
bool operator()(const intersc &a, const intersc &b) const
Definition of a sphere that bounds another object.
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
A basic definition of a shape. Shapes are considered centered at origin.
unsigned int vertexCount
the number of available vertices
TFSIMD_FORCE_INLINE tfScalar dot(const Vector3 &v) const
TFSIMD_FORCE_INLINE tfScalar distance2(const Vector3 &v) const
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool isPointInsidePlanes(const tf::Vector3 &point) const
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
TFSIMD_FORCE_INLINE Vector3 normalized() const
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
TFSIMD_FORCE_INLINE const tfScalar & y() const
double * vertices
the position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual void useDimensions(const shapes::Shape *shape)
TFSIMD_FORCE_INLINE Vector3 & normalize()
TFSIMD_FORCE_INLINE const tfScalar & w() const
intersc(const tf::Vector3 &_pt, const double _tm)
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
TFSIMD_FORCE_INLINE const tfScalar & getX() const
virtual void updateInternalData(void)
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
TFSIMD_FORCE_INLINE tfScalar length2() const
TFSIMD_FORCE_INLINE tfScalar length() const
TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const