38 #include <LinearMath/btConvexHull.h> 66 std::cerr <<
"Creating body from shape: Unknown shape type" << shape->
type << std::endl;
82 mergedSphere = spheres[0];
83 for (
unsigned int i = 1 ; i < spheres.size() ; ++i)
85 if (spheres[i].radius <= 0.0)
87 double d = spheres[i].center.distance(mergedSphere.
center);
88 if (d + mergedSphere.
radius <= spheres[i].radius)
90 mergedSphere.
center = spheres[i].center;
91 mergedSphere.
radius = spheres[i].radius;
94 if (d + spheres[i].radius > mergedSphere.
radius)
106 static const double ZERO = 1e-9;
113 double d = dir.
dot(a);
142 return (m_center - p).length2() < m_radius2;
152 m_radiusU = m_radius * m_scale + m_padding;
153 m_radius2 = m_radiusU * m_radiusU;
154 m_center = m_pose.getOrigin();
159 return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0;
165 sphere.
radius = m_radiusU;
170 if (
distanceSQR(m_center, origin, dir) > m_radius2)
return false;
175 double dpcpv = cp.
dot(dir);
179 double x = m_radius2 - w.length2();
184 double dpQv = w.
dot(dir);
188 intersections->push_back(Q);
199 double dpAv = w.
dot(dir);
201 double dpBv = w.dot(dir);
208 intersections->push_back(A);
218 intersections->push_back(B);
227 double pH = v.
dot(m_normalH);
229 if (fabs(pH) > m_length2)
232 double pB1 = v.
dot(m_normalB1);
233 double remaining = m_radius2 - pB1 * pB1;
239 double pB2 = v.
dot(m_normalB2);
240 return pB2 * pB2 < remaining;
252 m_radiusU = m_radius * m_scale + m_padding;
253 m_radius2 = m_radiusU * m_radiusU;
254 m_length2 = m_scale * m_length / 2.0 + m_padding;
255 m_center = m_pose.getOrigin();
256 m_radiusBSqr = m_length2 * m_length2 + m_radius2;
257 m_radiusB = sqrt(m_radiusBSqr);
264 double tmp = -m_normalH.
dot(m_center);
265 m_d1 = tmp + m_length2;
266 m_d2 = tmp - m_length2;
271 return 2.0 * M_PI * m_radius2 * m_length2;
277 sphere.
radius = m_radiusB;
282 if (
distanceSQR(m_center, origin, dir) > m_radiusBSqr)
return false;
284 std::vector<detail::intersc> ipts;
287 double tmp = m_normalH.dot(dir);
288 if (fabs(tmp) >
ZERO)
290 double tmp2 = -m_normalH.dot(origin);
291 double t1 = (tmp2 - m_d1) / tmp;
297 v1 = v1 - m_normalH.
dot(v1) * m_normalH;
300 if (intersections == NULL)
308 double t2 = (tmp2 - m_d2) / tmp;
313 v2 = v2 - m_normalH.
dot(v2) * m_normalH;
316 if (intersections == NULL)
329 tf::Vector3 ROD(m_normalH.cross(origin - m_center));
331 double b = 2.0 * ROD.dot(VD);
332 double c = ROD.length2() - m_radius2;
333 double d = b * b - 4.0 * a * c;
334 if (d > 0.0 && fabs(a) >
ZERO)
338 double t1 = (b + d) / e;
339 double t2 = (b - d) / e;
346 if (fabs(m_normalH.dot(v1)) < m_length2 +
ZERO)
348 if (intersections == NULL)
361 if (fabs(m_normalH.dot(v2)) < m_length2 +
ZERO)
363 if (intersections == NULL)
376 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
377 for (
unsigned int i = 0 ; i < n ; ++i)
378 intersections->push_back(ipts[i].pt);
388 double pL = v.
dot(m_normalL);
390 if (fabs(pL) > m_length2)
393 double pW = v.
dot(m_normalW);
395 if (fabs(pW) > m_width2)
398 double pH = v.
dot(m_normalH);
400 if (fabs(pH) > m_height2)
408 const double *size =
static_cast<const shapes::Box*
>(shape)->size;
416 double s2 = m_scale / 2.0;
417 m_length2 = m_length * s2 + m_padding;
418 m_width2 = m_width * s2 + m_padding;
419 m_height2 = m_height * s2 + m_padding;
421 m_center = m_pose.getOrigin();
423 m_radius2 = m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2;
424 m_radiusB = sqrt(m_radius2);
431 const tf::Vector3 tmp(m_normalL * m_length2 + m_normalW * m_width2 + m_normalH * m_height2);
432 m_corner1 = m_center - tmp;
433 m_corner2 = m_center + tmp;
438 return 8.0 * m_length2 * m_width2 * m_height2;
444 sphere.
radius = m_radiusB;
449 if (
distanceSQR(m_center, origin, dir) > m_radius2)
return false;
451 double t_near = -INFINITY;
452 double t_far = INFINITY;
454 for (
int i = 0; i < 3; i++)
456 const tf::Vector3 &vN = i == 0 ? m_normalL : (i == 1 ? m_normalW : m_normalH);
457 double dp = vN.
dot(dir);
461 double t1 = vN.
dot(m_corner1 - origin) / dp;
462 double t2 = vN.
dot(m_corner2 - origin) / dp;
483 if ((std::min(m_corner1.y(), m_corner2.y()) > origin.
y() ||
484 std::max(m_corner1.y(), m_corner2.y()) < origin.
y()) &&
485 (std::min(m_corner1.z(), m_corner2.z()) > origin.
z() ||
486 std::max(m_corner1.z(), m_corner2.z()) < origin.
z()))
493 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.
x() ||
494 std::max(m_corner1.x(), m_corner2.x()) < origin.
x()) &&
495 (std::min(m_corner1.z(), m_corner2.z()) > origin.
z() ||
496 std::max(m_corner1.z(), m_corner2.z()) < origin.
z()))
500 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.
x() ||
501 std::max(m_corner1.x(), m_corner2.x()) < origin.
x()) &&
502 (std::min(m_corner1.y(), m_corner2.y()) > origin.
y() ||
503 std::max(m_corner1.y(), m_corner2.y()) < origin.
y()))
511 if (t_far - t_near >
ZERO)
513 intersections->push_back(t_near * dir + origin);
515 intersections->push_back(t_far * dir + origin);
518 intersections->push_back(t_far * dir + origin);
710 if (m_boundingBox.containsPoint(p))
713 ip = m_meshCenter + (ip - m_meshCenter) * m_scale;
714 return isPointInsidePlanes(ip);
724 double maxX = -INFINITY, maxY = -INFINITY, maxZ = -INFINITY;
725 double minX = INFINITY, minY = INFINITY, minZ = INFINITY;
727 for(
unsigned int i = 0; i < mesh->
vertexCount ; ++i)
730 double vy = mesh->
vertices[3 * i + 1];
731 double vz = mesh->
vertices[3 * i + 2];
733 if (maxX < vx) maxX = vx;
734 if (maxY < vy) maxY = vy;
735 if (maxZ < vz) maxZ = vz;
737 if (minX > vx) minX = vx;
738 if (minY > vy) minY = vy;
739 if (minZ > vz) minZ = vz;
742 if (maxX < minX) maxX = minX = 0.0;
743 if (maxY < minY) maxY = minY = 0.0;
744 if (maxZ < minZ) maxZ = minZ = 0.0;
747 m_boundingBox.setDimensions(box_shape);
750 m_boxOffset.setValue((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
758 btVector3 *vertices =
new btVector3[mesh->
vertexCount];
759 for(
unsigned int i = 0; i < mesh->
vertexCount ; ++i)
761 vertices[i].setX(mesh->
vertices[3 * i ]);
762 vertices[i].setY(mesh->
vertices[3 * i + 1]);
763 vertices[i].setZ(mesh->
vertices[3 * i + 2]);
766 HullDesc hd(QF_TRIANGLES, mesh->
vertexCount, vertices);
769 if (hl.CreateConvexHull(hd, hr) == QE_OK)
773 m_vertices.reserve(hr.m_OutputVertices.size());
776 for (
int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
778 tf::Vector3 vector3_tmp(
tf::Vector3(hr.m_OutputVertices[j][0],hr.m_OutputVertices[j][1],hr.m_OutputVertices[j][2]));
779 m_vertices.push_back(vector3_tmp);
780 sum = sum + vector3_tmp;
783 m_meshCenter = sum / (double)(hr.m_OutputVertices.size());
784 for (
unsigned int j = 0 ; j < m_vertices.size() ; ++j)
786 double dist = m_vertices[j].
distance2(m_meshCenter);
787 if (dist > m_meshRadiusB)
788 m_meshRadiusB = dist;
790 m_meshRadiusB = sqrt(m_meshRadiusB);
792 m_triangles.reserve(hr.m_Indices.size());
793 for (
unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
795 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]);
796 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]);
797 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]);
812 unsigned int behindPlane = countVerticesBehindPlane(planeEquation);
815 tf::tfVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW());
816 unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2);
817 if (behindPlane2 < behindPlane)
819 planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW());
820 behindPlane = behindPlane2;
827 m_planes.push_back(planeEquation);
829 m_triangles.push_back(hr.m_Indices[j * 3 + 0]);
830 m_triangles.push_back(hr.m_Indices[j * 3 + 1]);
831 m_triangles.push_back(hr.m_Indices[j * 3 + 2]);
836 std::cerr <<
"Unable to compute convex hull.";
838 hl.ReleaseResult(hr);
847 m_boundingBox.setPose(pose);
848 m_boundingBox.setPadding(m_padding);
849 m_boundingBox.setScale(m_scale);
851 m_iPose = m_pose.inverse();
852 m_center = m_pose * m_meshCenter;
853 m_radiusB = m_meshRadiusB * m_scale + m_padding;
854 m_radiusBSqr = m_radiusB * m_radiusB;
856 m_scaledVertices.resize(m_vertices.size());
857 for (
unsigned int i = 0 ; i < m_vertices.size() ; ++i)
861 m_scaledVertices[i] = m_meshCenter + v * (m_scale + (l >
ZERO ? m_padding / l : 0.0));
868 sphere.
radius = m_radiusB;
873 unsigned int numplanes = m_planes.size();
874 for (
unsigned int i = 0 ; i < numplanes ; ++i)
878 if (dist > tfScalar(0))
886 unsigned int numvertices = m_vertices.size();
887 unsigned int result = 0;
888 for (
unsigned int i = 0 ; i < numvertices ; ++i)
900 for (
unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i)
902 const tf::Vector3 &v1 = m_vertices[m_triangles[3*i + 0]];
903 const tf::Vector3 &v2 = m_vertices[m_triangles[3*i + 1]];
904 const tf::Vector3 &v3 = m_vertices[m_triangles[3*i + 2]];
905 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();
907 return fabs(volume)/6.0;
912 if (
distanceSQR(m_center, origin, dir) > m_radiusBSqr)
return false;
913 if (!m_boundingBox.intersectsRay(origin, dir))
return false;
919 std::vector<detail::intersc> ipts;
924 const unsigned int nt = m_triangles.size() / 3;
925 for (
unsigned int i = 0 ; i < nt ; ++i)
928 if (fabs(tmp) >
ZERO)
930 double t = -(m_planes[i].dot(orig) + m_planes[i].getW()) / tmp;
933 const int i3 = 3 * i;
934 const int v1 = m_triangles[i3 + 0];
935 const int v2 = m_triangles[i3 + 1];
936 const int v3 = m_triangles[i3 + 2];
952 if (c1.dot(c2) < 0.0)
961 if (c1.dot(c2) < 0.0)
967 if (c1.dot(c2) < 0.0)
985 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
986 for (
unsigned int i = 0 ; i < n ; ++i)
987 intersections->push_back(ipts[i].pt);
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
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...
bool isPointInsidePlanes(const tf::Vector3 &point) const
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 computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition of a cylinder.
virtual void useDimensions(const shapes::Shape *shape)
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 bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
TFSIMD_FORCE_INLINE const tfScalar & getY() const
Definition of a cylinder.
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
intersc(const tf::Vector3 &_pt, const double _tm)
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
double * vertices
the position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
Definition of a sphere that bounds another object.
TFSIMD_FORCE_INLINE const tfScalar & x() const
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
TFSIMD_FORCE_INLINE tfScalar dot(const Vector3 &v) const
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
TFSIMD_FORCE_INLINE tfScalar distance2(const Vector3 &v) const
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
TFSIMD_FORCE_INLINE Vector3 normalized() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
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 void updateInternalData(void)
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool operator()(const intersc &a, const intersc &b) const
virtual void updateInternalData(void)
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
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...
TFSIMD_FORCE_INLINE Vector3 & normalize()
TFSIMD_FORCE_INLINE const tfScalar & w() const
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
A basic definition of a shape. Shapes are considered centered at origin.
virtual void useDimensions(const shapes::Shape *shape)
TFSIMD_FORCE_INLINE const tfScalar & getX() const
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
virtual void updateInternalData(void)
unsigned int vertexCount
the number of available vertices
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 void useDimensions(const shapes::Shape *shape)
TFSIMD_FORCE_INLINE tfScalar length2() const
virtual void useDimensions(const shapes::Shape *shape)
TFSIMD_FORCE_INLINE tfScalar length() const
TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const
virtual void updateInternalData(void)
unsigned int countVerticesBehindPlane(const tf::tfVector4 &planeNormal) const