atria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DEV] continue rework

Browse Source
This commit is contained in:
Edouard DUPIN 2018-06-15 21:17:25 +02:00
parent 696a979395
commit 4525e80a60
15 changed files with 538 additions and 642 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ephysics/collision/ContactManifold.hpp
View File

@ -93,6 +93,7 @@ namespace ephysics {
* only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
* this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
* by Erwin Coumans (http://wwww.bulletphysics.org).
*/
int32_t getIndexToRemove(int32_t _indexMaxPenetration, const vec3& _newPoint) const;
/// Remove a contact point from the manifold
void removeContactPoint(uint32_t _index);

2
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -93,7 +93,7 @@ void ConvexVsTriangleCallback::testTriangle(const vec3* _trianglePoints) {
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* _overlappingPair,
etk::Vector<SmoothMeshContactInfo> _contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) {
NarrowPhaseCallback* _callback) {
// Set with the triangle vertices already processed to void further contacts with same triangle
etk::Vector<etk::Pair<int32_t, vec3>> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth

2
ephysics/collision/shapes/CollisionShape.hpp
View File

@ -35,6 +35,8 @@ class CollisionShape {
CollisionShape(const CollisionShape& shape) = delete;
/// DELETE assignment operator
CollisionShape& operator=(const CollisionShape& shape) = delete;
/// Virtualize destructor
virtual ~CollisionShape() {};
/**
* @brief Get the type of the collision shapes
* @return The type of the collision shape (box, sphere, cylinder, ...)

2
ephysics/collision/shapes/ConcaveShape.hpp
View File

@ -31,8 +31,6 @@ namespace ephysics {
public :
/// Constructor
ConcaveShape(CollisionShapeType _type);
/// Destructor
virtual ~ConcaveShape();
/// DELETE copy-constructor
ConcaveShape(const ConcaveShape& _shape) = delete;
/// DELETE assignment operator

109
ephysics/collision/shapes/ConvexMeshShape.cpp
View File

@ -11,23 +11,31 @@
using namespace ephysics;
ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, float margin)
: ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) {
assert(nbVertices > 0);
assert(stride > 0);
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
ConvexMeshShape::ConvexMeshShape(const float* _arrayVertices,
uint32_t _nbVertices,
int32_t _stride,
float _margin):
ConvexShape(CONVEX_MESH, _margin),
m_numberVertices(_nbVertices),
m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0),
m_isEdgesInformationUsed(false) {
assert(_nbVertices > 0);
assert(_stride > 0);
const unsigned char* vertexPointer = (const unsigned char*) _arrayVertices;
// Copy all the vertices int32_to the int32_ternal array
for (uint32_t i=0; i<m_numberVertices; i++) {
for (uint32_t iii=0; iii<m_numberVertices; iii++) {
const float* newPoint = (const float*) vertexPointer;
m_vertices.pushBack(vec3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride;
vertexPointer += _stride;
}
// Recalculate the bounds of the mesh
recalculateBounds();
}
ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* _triangleVertexArray, bool _isEdgesInformationUsed, float _margin):
ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* _triangleVertexArray,
bool _isEdgesInformationUsed,
float _margin):
ConvexShape(CONVEX_MESH, _margin),
m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0),
@ -54,9 +62,6 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* _triangleVertexArray, bool
recalculateBounds();
}
// Constructor.
/// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(float _margin):
ConvexShape(CONVEX_MESH, _margin),
m_numberVertices(0),
@ -66,7 +71,8 @@ ConvexMeshShape::ConvexMeshShape(float _margin):
}
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& _direction,
void** _cachedCollisionData) const {
assert(m_numberVertices == m_vertices.size());
assert(_cachedCollisionData != nullptr);
// Allocate memory for the cached collision data if not allocated yet
@ -158,12 +164,12 @@ void ConvexMeshShape::recalculateBounds() {
m_minBounds -= vec3(m_margin, m_margin, m_margin);
}
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo);
bool ConvexMeshShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
return _proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(_ray, _proxyShape, _raycastInfo);
}
void ConvexMeshShape::setLocalScaling(const vec3& scaling) {
ConvexShape::setLocalScaling(scaling);
void ConvexMeshShape::setLocalScaling(const vec3& _scaling) {
ConvexShape::setLocalScaling(_scaling);
recalculateBounds();
}
@ -171,73 +177,72 @@ size_t ConvexMeshShape::getSizeInBytes() const {
return sizeof(ConvexMeshShape);
}
void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const {
min = m_minBounds;
max = m_maxBounds;
void ConvexMeshShape::getLocalBounds(vec3& _min, vec3& _max) const {
_min = m_minBounds;
_max = m_maxBounds;
}
void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float factor = (1.0f / float(3.0)) * mass;
void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const {
float factor = (1.0f / float(3.0)) * _mass;
vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds);
assert(realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0);
float xSquare = realExtent.x() * realExtent.x();
float ySquare = realExtent.y() * realExtent.y();
float zSquare = realExtent.z() * realExtent.z();
tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
_tensor.setValue(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
}
void ConvexMeshShape::addVertex(const vec3& vertex) {
void ConvexMeshShape::addVertex(const vec3& _vertex) {
// Add the vertex in to vertices array
m_vertices.pushBack(vertex);
m_vertices.pushBack(_vertex);
m_numberVertices++;
// Update the bounds of the mesh
if (vertex.x() * m_scaling.x() > m_maxBounds.x()) {
m_maxBounds.setX(vertex.x() * m_scaling.x());
if (_vertex.x() * m_scaling.x() > m_maxBounds.x()) {
m_maxBounds.setX(_vertex.x() * m_scaling.x());
}
if (vertex.x() * m_scaling.x() < m_minBounds.x()) {
m_minBounds.setX(vertex.x() * m_scaling.x());
if (_vertex.x() * m_scaling.x() < m_minBounds.x()) {
m_minBounds.setX(_vertex.x() * m_scaling.x());
}
if (vertex.y() * m_scaling.y() > m_maxBounds.y()) {
m_maxBounds.setY(vertex.y() * m_scaling.y());
if (_vertex.y() * m_scaling.y() > m_maxBounds.y()) {
m_maxBounds.setY(_vertex.y() * m_scaling.y());
}
if (vertex.y() * m_scaling.y() < m_minBounds.y()) {
m_minBounds.setY(vertex.y() * m_scaling.y());
if (_vertex.y() * m_scaling.y() < m_minBounds.y()) {
m_minBounds.setY(_vertex.y() * m_scaling.y());
}
if (vertex.z() * m_scaling.z() > m_maxBounds.z()) {
m_maxBounds.setZ(vertex.z() * m_scaling.z());
if (_vertex.z() * m_scaling.z() > m_maxBounds.z()) {
m_maxBounds.setZ(_vertex.z() * m_scaling.z());
}
if (vertex.z() * m_scaling.z() < m_minBounds.z()) {
m_minBounds.setZ(vertex.z() * m_scaling.z());
if (_vertex.z() * m_scaling.z() < m_minBounds.z()) {
m_minBounds.setZ(_vertex.z() * m_scaling.z());
}
}
void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) {
void ConvexMeshShape::addEdge(uint32_t _v1, uint32_t _v2) {
// If the entry for vertex v1 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v1) == 0) {
m_edgesAdjacencyList.add(v1, etk::Set<uint32_t>());
if (m_edgesAdjacencyList.count(_v1) == 0) {
m_edgesAdjacencyList.add(_v1, etk::Set<uint32_t>());
}
// If the entry for vertex v2 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v2) == 0) {
m_edgesAdjacencyList.add(v2, etk::Set<uint32_t>());
if (m_edgesAdjacencyList.count(_v2) == 0) {
m_edgesAdjacencyList.add(_v2, etk::Set<uint32_t>());
}
// Add the edge in the adjacency list
m_edgesAdjacencyList[v1].add(v2);
m_edgesAdjacencyList[v2].add(v1);
m_edgesAdjacencyList[_v1].add(_v2);
m_edgesAdjacencyList[_v2].add(_v1);
}
bool ConvexMeshShape::isEdgesInformationUsed() const {
return m_isEdgesInformationUsed;
}
void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
m_isEdgesInformationUsed = isEdgesUsed;
void ConvexMeshShape::setIsEdgesInformationUsed(bool _isEdgesUsed) {
m_isEdgesInformationUsed = _isEdgesUsed;
}
bool ConvexMeshShape::testPointInside(const vec3& localPoint,
ProxyShape* proxyShape) const {
bool ConvexMeshShape::testPointInside(const vec3& _localPoint,
ProxyShape* _proxyShape) const {
// Use the GJK algorithm to test if the point is inside the convex mesh
return proxyShape->m_body->m_world.m_collisionDetection.
m_narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
return _proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.testPointInside(_localPoint, _proxyShape);
}

10
ephysics/collision/shapes/ConvexMeshShape.hpp
View File

@ -54,7 +54,7 @@ namespace ephysics {
size_t getSizeInBytes() const override;
public :
/**
* @brief Constructor to initialize with an array of 3D vertices.
* @brief Constructor to initialize with an array of 3D vertices.
* This method creates an int32_ternal copy of the input vertices.
* @param[in] _arrayVertices Array with the vertices of the convex mesh
* @param[in] _nbVertices Number of vertices in the convex mesh
@ -75,8 +75,12 @@ namespace ephysics {
ConvexMeshShape(TriangleVertexArray* _triangleVertexArray,
bool _isEdgesInformationUsed = true,
float _margin = OBJECT_MARGIN);
/// Constructor.
/**
* @brief Constructor.
* If you use this constructor, you will need to set the vertices manually one by one using the addVertex() method.
*/
ConvexMeshShape(float _margin = OBJECT_MARGIN);
public:
void getLocalBounds(vec3& _min, vec3& _max) const override;
void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
/**
@ -86,7 +90,7 @@ namespace ephysics {
void addVertex(const vec3& _vertex);
/**
* @brief Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
* Note that the vertex indices start at zero and need to correspond to the order of
* @note that the vertex indices start at zero and need to correspond to the order of
* the vertices in the vertices array in the constructor or the order of the calls
* of the addVertex() methods that you use to add vertices int32_to the convex mesh.
* @param[in] _v1 Index of the first vertex of the edge to add

13
ephysics/collision/shapes/ConvexShape.hpp
View File

@ -14,23 +14,24 @@ namespace ephysics {
* @brief It represents a convex collision shape associated with a
* body that is used during the narrow-phase collision detection.
*/
class ConvexShape : public CollisionShape {
protected :
class ConvexShape: public CollisionShape {
protected:
float m_margin; //!< Margin used for the GJK collision detection algorithm
/// Private copy-constructor
ConvexShape(const ConvexShape& shape) = delete;
ConvexShape(const ConvexShape& _shape) = delete;
/// Private assignment operator
ConvexShape& operator=(const ConvexShape& shape) = delete;
ConvexShape& operator=(const ConvexShape& _shape) = delete;
// Return a local support point in a given direction with the object margin
virtual vec3 getLocalSupportPointWithMargin(const vec3& _direction, void** _cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const=0;
bool testPointInside(const vec3& _worldPoint, ProxyShape* _proxyShape) const override = 0;
public :
public:
/// Constructor
ConvexShape(CollisionShapeType type, float margin);
ConvexShape(CollisionShapeType _type, float _margin);
/// Destructor
virtual ~ConvexShape();
public:
/**
* @brief Get the current object margin
* @return The margin (in meters) around the collision shape

291
ephysics/collision/shapes/CylinderShape.cpp
View File

@ -12,21 +12,16 @@
using namespace ephysics;
/**
* @param radius Radius of the cylinder (in meters)
* @param height Height of the cylinder (in meters)
* @param margin Collision margin (in meters) around the collision shape
*/
CylinderShape::CylinderShape(float radius, float height, float margin)
: ConvexShape(CYLINDER, margin), mRadius(radius),
m_halfHeight(height/float(2.0)) {
assert(radius > 0.0f);
assert(height > 0.0f);
CylinderShape::CylinderShape(float _radius,
float _height,
float _margin):
ConvexShape(CYLINDER, _margin), m_radius(_radius), m_halfHeight(_height/float(2.0)) {
assert(_radius > 0.0f);
assert(_height > 0.0f);
}
// Return a local support point in a given direction without the object margin
vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction,
void** _cachedCollisionData) const {
vec3 supportPoint(0.0, 0.0, 0.0);
float uDotv = _direction.y();
vec3 w(_direction.x(), 0.0, _direction.z());
@ -37,7 +32,7 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, vo
} else {
supportPoint.setY(m_halfHeight);
}
supportPoint += (mRadius / lengthW) * w;
supportPoint += (m_radius / lengthW) * w;
} else {
if (uDotv < 0.0) {
supportPoint.setY(-m_halfHeight);
@ -48,234 +43,196 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, vo
return supportPoint;
}
// Raycast method with feedback information
/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by
/// Morgan Kaufmann.
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const vec3 n = ray.point2 - ray.point1;
bool CylinderShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
const vec3 n = _ray.point2 - _ray.point1;
const float epsilon = float(0.01);
vec3 p(float(0), -m_halfHeight, float(0));
vec3 q(float(0), m_halfHeight, float(0));
vec3 d = q - p;
vec3 m = ray.point1 - p;
vec3 m = _ray.point1 - p;
float t;
float mDotD = m.dot(d);
float nDotD = n.dot(d);
float dDotD = d.dot(d);
// Test if the segment is outside the cylinder
if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) return false;
if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) {
return false;
}
if (mDotD > dDotD && mDotD + nDotD > dDotD) {
return false;
}
float nDotN = n.dot(n);
float mDotN = m.dot(n);
float a = dDotD * nDotN - nDotD * nDotD;
float k = m.dot(m) - mRadius * mRadius;
float k = m.dot(m) - m_radius * m_radius;
float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis
if (etk::abs(a) < epsilon) {
// If the origin is outside the surface of the cylinder, we return no hit
if (c > 0.0f) return false;
// Here we know that the segment int32_tersect an endcap of the cylinder
// If the ray int32_tersects with the "p" endcap of the cylinder
if (mDotD < 0.0f) {
t = -mDotN / nDotN;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, float(-1), 0);
raycastInfo.worldNormal = normalDirection;
return true;
}
else if (mDotD > dDotD) { // If the ray int32_tersects with the "q" endcap of the cylinder
t = (nDotD - mDotN) / nDotN;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < 0.0f || t > ray.maxFraction) return false;
// Compute the hit information
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, 1.0f, 0);
raycastInfo.worldNormal = normalDirection;
return true;
}
else { // If the origin is inside the cylinder, we return no hit
if (c > 0.0f) {
return false;
}
// Here we know that the segment int32_tersect an endcap of the cylinder
// If the ray int32_tersects with the "p" endcap of the cylinder
if (mDotD < 0.0f) {
t = -mDotN / nDotN;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < 0.0f || t > _ray.maxFraction) {
return false;
}
// Compute the hit information
vec3 localHitPoint = _ray.point1 + t * n;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, float(-1), 0);
_raycastInfo.worldNormal = normalDirection;
return true;
}
// If the ray int32_tersects with the "q" endcap of the cylinder
if (mDotD > dDotD) {
t = (nDotD - mDotN) / nDotN;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < 0.0f || t > _ray.maxFraction) {
return false;
}
// Compute the hit information
vec3 localHitPoint = _ray.point1 + t * n;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, 1.0f, 0);
_raycastInfo.worldNormal = normalDirection;
return true;
}
// If the origin is inside the cylinder, we return no hit
return false;
}
float b = dDotD * mDotN - nDotD * mDotD;
float discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < 0.0f) return false;
if (discriminant < 0.0f) {
return false;
}
// Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - etk::sqrt(discriminant)) / a;
// If the int32_tersection is outside the cylinder on "p" endcap side
float value = mDotD + t * nDotD;
if (value < 0.0f) {
// If the ray is pointing away from the "p" endcap, we return no hit
if (nDotD <= 0.0f) return false;
if (nDotD <= 0.0f) {
return false;
}
// Compute the int32_tersection against the "p" endcap (int32_tersection agains whole plane)
t = -mDotD / nDotD;
// Keep the int32_tersection if the it is inside the cylinder radius
if (k + t * (float(2.0) * mDotN + t) > 0.0f) return false;
if (k + t * (float(2.0) * mDotN + t) > 0.0f) {
return false;
}
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < 0.0f || t > ray.maxFraction) return false;
if (t < 0.0f || t > _ray.maxFraction) {
return false;
}
// Compute the hit information
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
vec3 localHitPoint = _ray.point1 + t * n;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, float(-1.0), 0);
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
else if (value > dDotD) { // If the int32_tersection is outside the cylinder on the "q" side
// If the int32_tersection is outside the cylinder on the "q" side
if (value > dDotD) {
// If the ray is pointing away from the "q" endcap, we return no hit
if (nDotD >= 0.0f) return false;
if (nDotD >= 0.0f) {
return false;
}
// Compute the int32_tersection against the "q" endcap (int32_tersection against whole plane)
t = (dDotD - mDotD) / nDotD;
// Keep the int32_tersection if it is inside the cylinder radius
if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) >
0.0f) return false;
if (k + dDotD - float(2.0) * mDotD + t * (float(2.0) * (mDotN - nDotD) + t) > 0.0f) {
return false;
}
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < 0.0f || t > ray.maxFraction) return false;
if (t < 0.0f || t > _ray.maxFraction) {
return false;
}
// Compute the hit information
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
vec3 localHitPoint = _ray.point1 + t * n;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, 1.0f, 0);
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
t = t0;
// If the int32_tersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < 0.0f || t > ray.maxFraction) return false;
if (t < 0.0f || t > _ray.maxFraction) {
return false;
}
// Compute the hit information
vec3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
vec3 localHitPoint = _ray.point1 + t * n;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = localHitPoint;
vec3 v = localHitPoint - p;
vec3 w = (v.dot(d) / d.length2()) * d;
vec3 normalDirection = (localHitPoint - (p + w));
raycastInfo.worldNormal = normalDirection;
_raycastInfo.worldNormal = normalDirection;
return true;
}
// Return the radius
/**
* @return Radius of the cylinder (in meters)
*/
float CylinderShape::getRadius() const {
return mRadius;
return m_radius;
}
// Return the height
/**
* @return Height of the cylinder (in meters)
*/
float CylinderShape::getHeight() const {
return m_halfHeight + m_halfHeight;
}
// Set the scaling vector of the collision shape
void CylinderShape::setLocalScaling(const vec3& scaling) {
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y();
mRadius = (mRadius / m_scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling);
void CylinderShape::setLocalScaling(const vec3& _scaling) {
m_halfHeight = (m_halfHeight / m_scaling.y()) * _scaling.y();
m_radius = (m_radius / m_scaling.x()) * _scaling.x();
CollisionShape::setLocalScaling(_scaling);
}
// Return the number of bytes used by the collision shape
size_t CylinderShape::getSizeInBytes() const {
return sizeof(CylinderShape);
}
// Return the local bounds of the shape in x, y and z directions
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void CylinderShape::getLocalBounds(vec3& min, vec3& max) const {
void CylinderShape::getLocalBounds(vec3& _min, vec3& _max) const {
// Maximum bounds
max.setX(mRadius + m_margin);
max.setY(m_halfHeight + m_margin);
max.setZ(max.x());
_max.setX(m_radius + m_margin);
_max.setY(m_halfHeight + m_margin);
_max.setZ(_max.x());
// Minimum bounds
min.setX(-max.x());
min.setY(-max.y());
min.setZ(min.x());
_min.setX(-_max.x());
_min.setY(-_max.y());
_min.setZ(_min.x());
}
// Return the local inertia tensor of the cylinder
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const {
float height = float(2.0) * m_halfHeight;
float diag = (1.0f / float(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setValue(diag, 0.0, 0.0, 0.0,
0.5f * mass * mRadius * mRadius, 0.0,
0.0, 0.0, diag);
float diag = (1.0f / float(12.0)) * _mass * (3 * m_radius * m_radius + height * height);
_tensor.setValue(diag, 0.0, 0.0, 0.0,
0.5f * _mass * m_radius * m_radius, 0.0,
0.0, 0.0, diag);
}
// Return true if a point is inside the collision shape
bool CylinderShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const{
return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < mRadius * mRadius
&& localPoint.y() < m_halfHeight
&& localPoint.y() > -m_halfHeight);
bool CylinderShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const{
return ( (_localPoint.x() * _localPoint.x() + _localPoint.z() * _localPoint.z()) < m_radius * m_radius
&& _localPoint.y() < m_halfHeight
&& _localPoint.y() > -m_halfHeight);
}

83
ephysics/collision/shapes/CylinderShape.hpp
View File

@ -13,42 +13,53 @@
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
/**
* @brief It represents a cylinder collision shape around the Y axis
* and centered at the origin. The cylinder is defined by its height
* and the radius of its base. The "transform" of the corresponding
* rigid body gives an orientation and a position to the cylinder.
* This collision shape uses an extra margin distance around it for collision
* detection purpose. The default margin is 4cm (if your units are meters,
* which is recommended). In case, you want to simulate small objects
* (smaller than the margin distance), you might want to reduce the margin by
* specifying your own margin distance using the "margin" parameter in the
* constructor of the cylinder shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
*/
class CylinderShape : public ConvexShape {
protected :
float mRadius; //!< Radius of the base
float m_halfHeight; //!< Half height of the cylinder
/// Private copy-constructor
CylinderShape(const CylinderShape&) = delete;
/// Private assignment operator
CylinderShape& operator=(const CylinderShape&) = delete;
vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override;
public :
/// Constructor
CylinderShape(float _radius, float _height, float _margin = OBJECT_MARGIN);
/// Return the radius
float getRadius() const;
/// Return the height
float getHeight() const;
void setLocalScaling(const vec3& _scaling) override;
void getLocalBounds(vec3& _min, vec3& _max) const override;
void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
};
/**
* @brief It represents a cylinder collision shape around the Y axis
* and centered at the origin. The cylinder is defined by its height
* and the radius of its base. The "transform" of the corresponding
* rigid body gives an orientation and a position to the cylinder.
* This collision shape uses an extra margin distance around it for collision
* detection purpose. The default margin is 4cm (if your units are meters,
* which is recommended). In case, you want to simulate small objects
* (smaller than the margin distance), you might want to reduce the margin by
* specifying your own margin distance using the "margin" parameter in the
* constructor of the cylinder shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
*/
class CylinderShape: public ConvexShape {
protected:
float m_radius; //!< Radius of the base
float m_halfHeight; //!< Half height of the cylinder
/// DELETED copy-constructor
CylinderShape(const CylinderShape&) = delete;
/// DELETED assignment operator
CylinderShape& operator=(const CylinderShape&) = delete;
vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override;
public:
/**
* @brief Contructor
* @param radius Radius of the cylinder (in meters)
* @param height Height of the cylinder (in meters)
* @param margin Collision margin (in meters) around the collision shape
*/
CylinderShape(float _radius, float _height, float _margin = OBJECT_MARGIN);
/**
* @breif Get the Shape radius
* @return Radius of the cylinder (in meters)
*/
float getRadius() const;
/**
* @breif Get the Shape height
* @return Height of the cylinder (in meters)
*/
float getHeight() const;
void setLocalScaling(const vec3& _scaling) override;
void getLocalBounds(vec3& _min, vec3& _max) const override;
void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
};
}

324
ephysics/collision/shapes/HeightFieldShape.cpp
View File

@ -8,228 +8,183 @@
*/
#include <ephysics/collision/shapes/HeightFieldShape.hpp>
// TODO: REMOVE this...
using namespace ephysics;
/**
* @param nbGridColumns Number of columns in the grid of the height field
* @param nbGridRows Number of rows in the grid of the height field
* @param minHeight Minimum height value of the height field
* @param maxHeight Maximum height value of the height field
* @param heightFieldData Pointer to the first height value data (note that values are shared and not copied)
* @param dataType Data type for the height values (int32_t, float, double)
* @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
* @param int32_tegerHeightScale Scaling factor used to scale the height values (only when height values type is int32_teger)
*/
HeightFieldShape::HeightFieldShape(int32_t nbGridColumns, int32_t nbGridRows, float minHeight, float maxHeight,
const void* heightFieldData, HeightDataType dataType, int32_t upAxis,
float int32_tegerHeightScale)
: ConcaveShape(HEIGHTFIELD), m_numberColumns(nbGridColumns), m_numberRows(nbGridRows),
m_width(nbGridColumns - 1), m_length(nbGridRows - 1), m_minHeight(minHeight),
m_maxHeight(maxHeight), m_upAxis(upAxis), m_integerHeightScale(int32_tegerHeightScale),
m_heightDataType(dataType) {
assert(nbGridColumns >= 2);
assert(nbGridRows >= 2);
HeightFieldShape::HeightFieldShape(int32_t _nbGridColumns,
int32_t _nbGridRows,
float _minHeight,
float _maxHeight,
const void* _heightFieldData,
HeightDataType _dataType,
int32_t _upAxis,
float _integerHeightScale):
ConcaveShape(HEIGHTFIELD),
m_numberColumns(_nbGridColumns),
m_numberRows(_nbGridRows),
m_width(_nbGridColumns - 1),
m_length(_nbGridRows - 1),
m_minHeight(_minHeight),
m_maxHeight(_maxHeight),
m_upAxis(_upAxis),
m_integerHeightScale(_integerHeightScale),
m_heightDataType(_dataType) {
assert(_nbGridColumns >= 2);
assert(_nbGridRows >= 2);
assert(m_width >= 1);
assert(m_length >= 1);
assert(minHeight <= maxHeight);
assert(upAxis == 0 || upAxis == 1 || upAxis == 2);
m_heightFieldData = heightFieldData;
assert(_minHeight <= _maxHeight);
assert(_upAxis == 0 || _upAxis == 1 || _upAxis == 2);
m_heightFieldData = _heightFieldData;
float halfHeight = (m_maxHeight - m_minHeight) * 0.5f;
assert(halfHeight >= 0);
// Compute the local AABB of the height field
if (m_upAxis == 0) {
m_AABB.setMin(vec3(-halfHeight, -m_width * 0.5f, -m_length * float(0.5)));
m_AABB.setMax(vec3(halfHeight, m_width * 0.5f, m_length* float(0.5)));
}
else if (m_upAxis == 1) {
} else if (m_upAxis == 1) {
m_AABB.setMin(vec3(-m_width * 0.5f, -halfHeight, -m_length * float(0.5)));
m_AABB.setMax(vec3(m_width * 0.5f, halfHeight, m_length * float(0.5)));
}
else if (m_upAxis == 2) {
} else if (m_upAxis == 2) {
m_AABB.setMin(vec3(-m_width * 0.5f, -m_length * float(0.5), -halfHeight));
m_AABB.setMax(vec3(m_width * 0.5f, m_length * float(0.5), halfHeight));
}
}
// Return the local bounds of the shape in x, y and z directions.
// This method is used to compute the AABB of the box
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void HeightFieldShape::getLocalBounds(vec3& min, vec3& max) const {
min = m_AABB.getMin() * m_scaling;
max = m_AABB.getMax() * m_scaling;
void HeightFieldShape::getLocalBounds(vec3& _min, vec3& _max) const {
_min = m_AABB.getMin() * m_scaling;
_max = m_AABB.getMax() * m_scaling;
}
// Test collision with the triangles of the height field shape. The idea is to use the AABB
// of the body when need to test and see against which triangles of the height-field we need
// to test for collision. We compute the sub-grid points that are inside the other body's AABB
// and then for each rectangle in the sub-grid we generate two triangles that we use to test collision.
void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
// Compute the non-scaled AABB
vec3 inverseScaling(1.0f / m_scaling.x(), 1.0f / m_scaling.y(), float(1.0) / m_scaling.z());
AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
// Compute the int32_teger grid coordinates inside the area we need to test for collision
int32_t minGridCoords[3];
int32_t maxGridCoords[3];
computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
// Compute the starting and ending coords of the sub-grid according to the up axis
int32_t iMin = 0;
int32_t iMax = 0;
int32_t jMin = 0;
int32_t jMax = 0;
switch(m_upAxis) {
case 0 : iMin = clamp(minGridCoords[1], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[1], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[2], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1);
break;
case 1 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[2], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1);
break;
case 2 : iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[1], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[1], 0, m_numberRows - 1);
break;
}
assert(iMin >= 0 && iMin < m_numberColumns);
assert(iMax >= 0 && iMax < m_numberColumns);
assert(jMin >= 0 && jMin < m_numberRows);
assert(jMax >= 0 && jMax < m_numberRows);
// For each sub-grid points (except the last ones one each dimension)
for (int32_t i = iMin; i < iMax; i++) {
for (int32_t j = jMin; j < jMax; j++) {
// Compute the four point of the current quad
vec3 p1 = getVertexAt(i, j);
vec3 p2 = getVertexAt(i, j + 1);
vec3 p3 = getVertexAt(i + 1, j);
vec3 p4 = getVertexAt(i + 1, j + 1);
// Generate the first triangle for the current grid rectangle
vec3 trianglePoints[3] = {p1, p2, p3};
// Test collision against the first triangle
callback.testTriangle(trianglePoints);
// Generate the second triangle for the current grid rectangle
trianglePoints[0] = p3;
trianglePoints[1] = p2;
trianglePoints[2] = p4;
// Test collision against the second triangle
callback.testTriangle(trianglePoints);
}
}
void HeightFieldShape::testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const {
// Compute the non-scaled AABB
vec3 inverseScaling(1.0f / m_scaling.x(), 1.0f / m_scaling.y(), float(1.0) / m_scaling.z());
AABB aabb(_localAABB.getMin() * inverseScaling, _localAABB.getMax() * inverseScaling);
// Compute the int32_teger grid coordinates inside the area we need to test for collision
int32_t minGridCoords[3];
int32_t maxGridCoords[3];
computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
// Compute the starting and ending coords of the sub-grid according to the up axis
int32_t iMin = 0;
int32_t iMax = 0;
int32_t jMin = 0;
int32_t jMax = 0;
switch(m_upAxis) {
case 0 :
iMin = clamp(minGridCoords[1], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[1], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[2], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1);
break;
case 1 :
iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[2], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[2], 0, m_numberRows - 1);
break;
case 2 :
iMin = clamp(minGridCoords[0], 0, m_numberColumns - 1);
iMax = clamp(maxGridCoords[0], 0, m_numberColumns - 1);
jMin = clamp(minGridCoords[1], 0, m_numberRows - 1);
jMax = clamp(maxGridCoords[1], 0, m_numberRows - 1);
break;
}
assert(iMin >= 0 && iMin < m_numberColumns);
assert(iMax >= 0 && iMax < m_numberColumns);
assert(jMin >= 0 && jMin < m_numberRows);
assert(jMax >= 0 && jMax < m_numberRows);
// For each sub-grid points (except the last ones one each dimension)
for (int32_t i = iMin; i < iMax; i++) {
for (int32_t j = jMin; j < jMax; j++) {
// Compute the four point of the current quad
vec3 p1 = getVertexAt(i, j);
vec3 p2 = getVertexAt(i, j + 1);
vec3 p3 = getVertexAt(i + 1, j);
vec3 p4 = getVertexAt(i + 1, j + 1);
// Generate the first triangle for the current grid rectangle
vec3 trianglePoints[3] = {p1, p2, p3};
// Test collision against the first triangle
_callback.testTriangle(trianglePoints);
// Generate the second triangle for the current grid rectangle
trianglePoints[0] = p3;
trianglePoints[1] = p2;
trianglePoints[2] = p4;
// Test collision against the second triangle
_callback.testTriangle(trianglePoints);
}
}
}
// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and
// the AABB to collide
void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* minCoords, int32_t* maxCoords, const AABB& aabbToCollide) const {
void HeightFieldShape::computeMinMaxGridCoordinates(int32_t* _minCoords, int32_t* _maxCoords, const AABB& _aabbToCollide) const {
// Clamp the min/max coords of the AABB to collide inside the height field AABB
vec3 minPoint = etk::max(aabbToCollide.getMin(), m_AABB.getMin());
vec3 minPoint = etk::max(_aabbToCollide.getMin(), m_AABB.getMin());
minPoint = etk::min(minPoint, m_AABB.getMax());
vec3 maxPoint = etk::min(aabbToCollide.getMax(), m_AABB.getMax());
vec3 maxPoint = etk::min(_aabbToCollide.getMax(), m_AABB.getMax());
maxPoint = etk::max(maxPoint, m_AABB.getMin());
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... m_width/2]
// and [-m_length/2 ... m_length/2]
const vec3 translateVec = m_AABB.getExtent() * 0.5f;
minPoint += translateVec;
maxPoint += translateVec;
// Convert the floating min/max coords of the AABB int32_to closest int32_teger
// grid values (note that we use the closest grid coordinate that is out
// of the AABB)
minCoords[0] = computeIntegerGridValue(minPoint.x()) - 1;
minCoords[1] = computeIntegerGridValue(minPoint.y()) - 1;
minCoords[2] = computeIntegerGridValue(minPoint.z()) - 1;
maxCoords[0] = computeIntegerGridValue(maxPoint.x()) + 1;
maxCoords[1] = computeIntegerGridValue(maxPoint.y()) + 1;
maxCoords[2] = computeIntegerGridValue(maxPoint.z()) + 1;
_minCoords[0] = computeIntegerGridValue(minPoint.x()) - 1;
_minCoords[1] = computeIntegerGridValue(minPoint.y()) - 1;
_minCoords[2] = computeIntegerGridValue(minPoint.z()) - 1;
_maxCoords[0] = computeIntegerGridValue(maxPoint.x()) + 1;
_maxCoords[1] = computeIntegerGridValue(maxPoint.y()) + 1;
_maxCoords[2] = computeIntegerGridValue(maxPoint.z()) + 1;
}
// Raycast method with feedback information
/// Note that only the first triangle hit by the ray in the mesh will be returned, even if
/// the ray hits many triangles.
bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool HeightFieldShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
// TODO : Implement raycasting without using an AABB for the ray
// but using a dynamic AABB tree or octree instead
PROFILE("HeightFieldShape::raycast()");
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
TriangleOverlapCallback triangleCallback(_ray, _proxyShape, _raycastInfo, *this);
// Compute the AABB for the ray
const vec3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const AABB rayAABB(etk::min(ray.point1, rayEnd), etk::max(ray.point1, rayEnd));
const vec3 rayEnd = _ray.point1 + _ray.maxFraction * (_ray.point2 - _ray.point1);
const AABB rayAABB(etk::min(_ray.point1, rayEnd), etk::max(_ray.point1, rayEnd));
testAllTriangles(triangleCallback, rayAABB);
return triangleCallback.getIsHit();
}
// Return the vertex (local-coordinates) of the height field at a given (x,y) position
vec3 HeightFieldShape::getVertexAt(int32_t x, int32_t y) const {
vec3 HeightFieldShape::getVertexAt(int32_t _xxx, int32_t _yyy) const {
// Get the height value
const float height = getHeightAt(x, y);
const float height = getHeightAt(_xxx, _yyy);
// Height values origin
const float heightOrigin = -(m_maxHeight - m_minHeight) * 0.5f - m_minHeight;
vec3 vertex;
switch (m_upAxis) {
case 0: vertex = vec3(heightOrigin + height, -m_width * 0.5f + x, -m_length * float(0.5) + y);
break;
case 1: vertex = vec3(-m_width * 0.5f + x, heightOrigin + height, -m_length * float(0.5) + y);
break;
case 2: vertex = vec3(-m_width * 0.5f + x, -m_length * float(0.5) + y, heightOrigin + height);
break;
default: assert(false);
case 0:
vertex = vec3(heightOrigin + height, -m_width * 0.5f + _xxx, -m_length * float(0.5) + _yyy);
break;
case 1:
vertex = vec3(-m_width * 0.5f + _xxx, heightOrigin + height, -m_length * float(0.5) + _yyy);
break;
case 2:
vertex = vec3(-m_width * 0.5f + _xxx, -m_length * float(0.5) + _yyy, heightOrigin + height);
break;
default:
assert(false);
}
assert(m_AABB.contains(vertex));
return vertex * m_scaling;
}
// Raycast test between a ray and a triangle of the heightfield
void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) {
void TriangleOverlapCallback::testTriangle(const vec3* _trianglePoints) {
// Create a triangle collision shape
float margin = m_heightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
TriangleShape triangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin);
triangleShape.setRaycastTestType(m_heightFieldShape.getRaycastTestType());
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(m_ray, raycastInfo, m_proxyShape);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= m_smallestHitFraction) {
if ( isTriangleHit
&& raycastInfo.hitFraction <= m_smallestHitFraction) {
assert(raycastInfo.hitFraction >= 0.0f);
m_raycastInfo.body = raycastInfo.body;
m_raycastInfo.proxyShape = raycastInfo.proxyShape;
m_raycastInfo.hitFraction = raycastInfo.hitFraction;
@ -237,66 +192,55 @@ void TriangleOverlapCallback::testTriangle(const vec3* trianglePoints) {
m_raycastInfo.worldNormal = raycastInfo.worldNormal;
m_raycastInfo.meshSubpart = -1;
m_raycastInfo.triangleIndex = -1;
m_smallestHitFraction = raycastInfo.hitFraction;
m_isHit = true;
}
}
// Return the number of rows in the height field
int32_t HeightFieldShape::getNbRows() const {
return m_numberRows;
}
// Return the number of columns in the height field
int32_t HeightFieldShape::getNbColumns() const {
return m_numberColumns;
}
// Return the type of height value in the height field
HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const {
return m_heightDataType;
}
// Return the number of bytes used by the collision shape
size_t HeightFieldShape::getSizeInBytes() const {
return sizeof(HeightFieldShape);
}
// Set the local scaling vector of the collision shape
void HeightFieldShape::setLocalScaling(const vec3& scaling) {
CollisionShape::setLocalScaling(scaling);
void HeightFieldShape::setLocalScaling(const vec3& _scaling) {
CollisionShape::setLocalScaling(_scaling);
}
// Return the height of a given (x,y) point in the height field
float HeightFieldShape::getHeightAt(int32_t x, int32_t y) const {
float HeightFieldShape::getHeightAt(int32_t _xxx, int32_t _yyy) const {
switch(m_heightDataType) {
case HEIGHT_FLOAT_TYPE : return ((float*)m_heightFieldData)[y * m_numberColumns + x];
case HEIGHT_DOUBLE_TYPE : return ((double*)m_heightFieldData)[y * m_numberColumns + x];
case HEIGHT_INT_TYPE : return ((int32_t*)m_heightFieldData)[y * m_numberColumns + x] * m_integerHeightScale;
default: assert(false); return 0;
case HEIGHT_FLOAT_TYPE:
return ((float*)m_heightFieldData)[_yyy * m_numberColumns + _xxx];
case HEIGHT_DOUBLE_TYPE:
return ((double*)m_heightFieldData)[_yyy * m_numberColumns + _xxx];
case HEIGHT_INT_TYPE:
return ((int32_t*)m_heightFieldData)[_yyy * m_numberColumns + _xxx] * m_integerHeightScale;
default:
assert(false);
return 0;
}
}
// Return the closest inside int32_teger grid value of a given floating grid value
int32_t HeightFieldShape::computeIntegerGridValue(float value) const {
return (value < 0.0f) ? value - 0.5f : value + float(0.5);
int32_t HeightFieldShape::computeIntegerGridValue(float _value) const {
return (_value < 0.0f) ? _value - 0.5f : _value + 0.5f;
}
// Return the local inertia tensor
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
tensor.setValue(mass, 0, 0,
0, mass, 0,
0, 0, mass);
_tensor.setValue(_mass, 0, 0,
0, _mass, 0,
0, 0, _mass);
}

16
ephysics/collision/shapes/HeightFieldShape.hpp
View File

@ -77,9 +77,9 @@ namespace ephysics {
HeightDataType m_heightDataType; //!< Data type of the height values
const void* m_heightFieldData; //!< Array of data with all the height values of the height field
AABB m_AABB; //!< Local AABB of the height field (without scaling)
/// Private copy-constructor
/// DELETED copy-constructor
HeightFieldShape(const HeightFieldShape&) = delete;
/// Private assignment operator
/// DELETED assignment operator
HeightFieldShape& operator=(const HeightFieldShape&) = delete;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override;
@ -99,7 +99,17 @@ namespace ephysics {
/// Compute the min/max grid coords corresponding to the int32_tersection of the AABB of the height field and the AABB to collide
void computeMinMaxGridCoordinates(int32_t* _minCoords, int32_t* _maxCoords, const AABB& _aabbToCollide) const;
public:
/// Constructor
/**
* @brief Contructor
* @param nbGridColumns Number of columns in the grid of the height field
* @param nbGridRows Number of rows in the grid of the height field
* @param minHeight Minimum height value of the height field
* @param maxHeight Maximum height value of the height field
* @param heightFieldData Pointer to the first height value data (note that values are shared and not copied)
* @param dataType Data type for the height values (int32_t, float, double)
* @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
* @param int32_tegerHeightScale Scaling factor used to scale the height values (only when height values type is int32_teger)
*/
HeightFieldShape(int32_t _nbGridColumns,
int32_t _nbGridRows,
float _minHeight,

57
ephysics/collision/shapes/SphereShape.cpp
View File

@ -10,17 +10,14 @@
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp>
// TODO: REMOVE this ...
using namespace ephysics;
// Constructor
/**
* @param radius Radius of the sphere (in meters)
*/
SphereShape::SphereShape(float radius) : ConvexShape(SPHERE, radius) {
assert(radius > 0.0f);
SphereShape::SphereShape(float _radius):
ConvexShape(SPHERE, _radius) {
assert(_radius > 0.0f);
}
void SphereShape::setLocalScaling(const vec3& _scaling) {
m_margin = (m_margin / m_scaling.x()) * _scaling.x();
CollisionShape::setLocalScaling(_scaling);
@ -52,49 +49,41 @@ void SphereShape::getLocalBounds(vec3& _min, vec3& _max) const {
_min.setZ(_min.x());
}
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const vec3 m = ray.point1;
bool SphereShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
const vec3 m = _ray.point1;
float c = m.dot(m) - m_margin * m_margin;
// If the origin of the ray is inside the sphere, we return no int32_tersection
if (c < 0.0f) return false;
const vec3 rayDirection = ray.point2 - ray.point1;
if (c < 0.0f) {
return false;
}
const vec3 rayDirection = _ray.point2 - _ray.point1;
float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no int32_tersection
if (b > 0.0f) return false;
if (b > 0.0f) {
return false;
}
float raySquareLength = rayDirection.length2();
// Compute the discriminant of the quadratic equation
float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < 0.0f || raySquareLength < FLT_EPSILON) return false;
if ( discriminant < 0.0f
|| raySquareLength < FLT_EPSILON) {
return false;
}
// Compute the solution "t" closest to the origin
float t = -b - etk::sqrt(discriminant);
assert(t >= 0.0f);
// If the hit point is withing the segment ray fraction
if (t < ray.maxFraction * raySquareLength) {
if (t < _ray.maxFraction * raySquareLength) {
// Compute the int32_tersection information
t /= raySquareLength;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = ray.point1 + t * rayDirection;
raycastInfo.worldNormal = raycastInfo.worldPoint;
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = _ray.point1 + t * rayDirection;
_raycastInfo.worldNormal = _raycastInfo.worldPoint;
return true;
}
return false;
}

5
ephysics/collision/shapes/SphereShape.hpp
View File

@ -36,7 +36,10 @@ namespace ephysics {
return sizeof(SphereShape);
}
public :
/// Constructor
/**
* @brief Constructor
* @param[in] radius Radius of the sphere (in meters)
*/
SphereShape(float _radius);
/**
* @brief Get the radius of the sphere

217
ephysics/collision/shapes/TriangleShape.cpp
View File

@ -11,194 +11,153 @@
#include <ephysics/engine/Profiler.hpp>
#include <ephysics/configuration.hpp>
// TODO: REMOVE this...
using namespace ephysics;
// Constructor
/**
* @param point1 First point of the triangle
* @param point2 Second point of the triangle
* @param point3 Third point of the triangle
* @param margin The collision margin (in meters) around the collision shape
*/
TriangleShape::TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3, float margin)
: ConvexShape(TRIANGLE, margin) {
m_points[0] = point1;
m_points[1] = point2;
m_points[2] = point3;
TriangleShape::TriangleShape(const vec3& _point1, const vec3& _point2, const vec3& _point3, float _margin)
: ConvexShape(TRIANGLE, _margin) {
m_points[0] = _point1;
m_points[1] = _point2;
m_points[2] = _point3;
m_raycastTestType = FRONT;
}
// Destructor
TriangleShape::~TriangleShape() {
}
// Raycast method with feedback information
/// This method use the line vs triangle raycasting technique described in
/// Real-time Collision Detection by Christer Ericson.
bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool TriangleShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
PROFILE("TriangleShape::raycast()");
const vec3 pq = ray.point2 - ray.point1;
const vec3 pa = m_points[0] - ray.point1;
const vec3 pb = m_points[1] - ray.point1;
const vec3 pc = m_points[2] - ray.point1;
const vec3 pq = _ray.point2 - _ray.point1;
const vec3 pa = m_points[0] - _ray.point1;
const vec3 pb = m_points[1] - _ray.point1;
const vec3 pc = m_points[2] - _ray.point1;
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
// product for this test.
const vec3 m = pq.cross(pc);
float u = pb.dot(m);
if (m_raycastTestType == FRONT) {
if (u < 0.0f) return false;
if (u < 0.0f) {
return false;
}
} else if (m_raycastTestType == BACK) {
if (u > 0.0f) {
return false;
}
}
else if (m_raycastTestType == BACK) {
if (u > 0.0f) return false;
}
float v = -pa.dot(m);
if (m_raycastTestType == FRONT) {
if (v < 0.0f) return false;
if (v < 0.0f) {
return false;
}
} else if (m_raycastTestType == BACK) {
if (v > 0.0f) {
return false;
}
} else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, v)) {
return false;
}
}
else if (m_raycastTestType == BACK) {
if (v > 0.0f) return false;
}
else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, v)) return false;
}
float w = pa.dot(pq.cross(pb));
if (m_raycastTestType == FRONT) {
if (w < 0.0f) return false;
if (w < 0.0f) {
return false;
}
} else if (m_raycastTestType == BACK) {
if (w > 0.0f) {
return false;
}
} else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, w)) {
return false;
}
}
else if (m_raycastTestType == BACK) {
if (w > 0.0f) return false;
}
else if (m_raycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, w)) return false;
}
// If the line PQ is in the triangle plane (case where u=v=w=0)
if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false;
if ( approxEqual(u, 0)
&& approxEqual(v, 0)
&& approxEqual(w, 0)) {
return false;
}
// Compute the barycentric coordinates (u, v, w) to determine the
// int32_tersection point R, R = u * a + v * b + w * c
float denom = 1.0f / (u + v + w);
u *= denom;
v *= denom;
w *= denom;
// Compute the local hit point using the barycentric coordinates
const vec3 localHitPoint = u * m_points[0] + v * m_points[1] + w * m_points[2];
const float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
if (hitFraction < 0.0f || hitFraction > ray.maxFraction) return false;
const float hitFraction = (localHitPoint - _ray.point1).length() / pq.length();
if ( hitFraction < 0.0f
|| hitFraction > _ray.maxFraction) {
return false;
}
vec3 localHitNormal = (m_points[1] - m_points[0]).cross(m_points[2] - m_points[0]);
if (localHitNormal.dot(pq) > 0.0f) localHitNormal = -localHitNormal;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.worldPoint = localHitPoint;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldNormal = localHitNormal;
if (localHitNormal.dot(pq) > 0.0f) {
localHitNormal = -localHitNormal;
}
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.worldPoint = localHitPoint;
_raycastInfo.hitFraction = hitFraction;
_raycastInfo.worldNormal = localHitNormal;
return true;
}
// Return the number of bytes used by the collision shape
size_t TriangleShape::getSizeInBytes() const {
return sizeof(TriangleShape);
}
// Return a local support point in a given direction without the object margin
vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
vec3 dotProducts(direction.dot(m_points[0]), direction.dot(m_points[1]), direction.dot(m_points[2]));
vec3 TriangleShape::getLocalSupportPointWithoutMargin(const vec3& _direction,
void** _cachedCollisionData) const {
vec3 dotProducts(_direction.dot(m_points[0]), _direction.dot(m_points[1]), _direction.dot(m_points[2]));
return m_points[dotProducts.getMaxAxis()];
}
// Return the local bounds of the shape in x, y and z directions.
// This method is used to compute the AABB of the box
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void TriangleShape::getLocalBounds(vec3& min, vec3& max) const {
void TriangleShape::getLocalBounds(vec3& _min, vec3& _max) const {
const vec3 xAxis(m_points[0].x(), m_points[1].x(), m_points[2].x());
const vec3 yAxis(m_points[0].y(), m_points[1].y(), m_points[2].y());
const vec3 zAxis(m_points[0].z(), m_points[1].z(), m_points[2].z());
min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
min -= vec3(m_margin, m_margin, m_margin);
max += vec3(m_margin, m_margin, m_margin);
_min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
_max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
_min -= vec3(m_margin, m_margin, m_margin);
_max += vec3(m_margin, m_margin, m_margin);
}
// Set the local scaling vector of the collision shape
void TriangleShape::setLocalScaling(const vec3& scaling) {
m_points[0] = (m_points[0] / m_scaling) * scaling;
m_points[1] = (m_points[1] / m_scaling) * scaling;
m_points[2] = (m_points[2] / m_scaling) * scaling;
CollisionShape::setLocalScaling(scaling);
void TriangleShape::setLocalScaling(const vec3& _scaling) {
m_points[0] = (m_points[0] / m_scaling) * _scaling;
m_points[1] = (m_points[1] / m_scaling) * _scaling;
m_points[2] = (m_points[2] / m_scaling) * _scaling;
CollisionShape::setLocalScaling(_scaling);
}
// Return the local inertia tensor of the triangle shape
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
tensor.setZero();
void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const {
_tensor.setZero();
}
// Update the AABB of a body using its collision shape
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
* @param transform etk::Transform3D used to compute the AABB of the collision shape
*/
void TriangleShape::computeAABB(AABB& aabb, const etk::Transform3D& transform) const {
const vec3 worldPoint1 = transform * m_points[0];
const vec3 worldPoint2 = transform * m_points[1];
const vec3 worldPoint3 = transform * m_points[2];
void TriangleShape::computeAABB(AABB& _aabb, const etk::Transform3D& _transform) const {
const vec3 worldPoint1 = _transform * m_points[0];
const vec3 worldPoint2 = _transform * m_points[1];
const vec3 worldPoint3 = _transform * m_points[2];
const vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
const vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
const vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
_aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
_aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
}
// Return true if a point is inside the collision shape
bool TriangleShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
bool TriangleShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const {
return false;
}
// Return the raycast test type (front, back, front-back)
TriangleRaycastSide TriangleShape::getRaycastTestType() const {
return m_raycastTestType;
}
// Set the raycast test type (front, back, front-back)
/**
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
m_raycastTestType = testType;
void TriangleShape::setRaycastTestType(TriangleRaycastSide _testType) {
m_raycastTestType = _testType;
}
// Return the coordinates of a given vertex of the triangle
/**
* @param index Index (0 to 2) of a vertex of the triangle
*/
vec3 TriangleShape::getVertex(int32_t index) const {
assert(index >= 0 && index < 3);
return m_points[index];
vec3 TriangleShape::getVertex(int32_t _index) const {
assert( _index >= 0
&& _index < 3);
return m_points[_index];
}

46
ephysics/collision/shapes/TriangleShape.hpp
View File

@ -25,34 +25,46 @@ namespace ephysics {
* This class represents a triangle collision shape that is centered
* at the origin and defined three points.
*/
class TriangleShape : public ConvexShape {
class TriangleShape: public ConvexShape {
protected:
vec3 m_points[3]; //!< Three points of the triangle
TriangleRaycastSide m_raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
/// Private copy-constructor
TriangleShape(const TriangleShape& shape);
TriangleShape(const TriangleShape& _shape);
/// Private assignment operator
TriangleShape& operator=(const TriangleShape& shape);
TriangleShape& operator=(const TriangleShape& _shape);
vec3 getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const override;
bool testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const override;
bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override;
public:
/// Constructor
TriangleShape(const vec3& point1, const vec3& point2, const vec3& point3,
float margin = OBJECT_MARGIN);
/// Destructor
virtual ~TriangleShape();
void getLocalBounds(vec3& min, vec3& max) const override;
void setLocalScaling(const vec3& scaling) override;
void computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const override;
void computeAABB(AABB& aabb, const etk::Transform3D& transform) const override;
/**
* @brief Constructor
* @param _point1 First point of the triangle
* @param _point2 Second point of the triangle
* @param _point3 Third point of the triangle
* @param _margin The collision margin (in meters) around the collision shape
*/
TriangleShape(const vec3& _point1,
const vec3& _point2,
const vec3& _point3,
float _margin = OBJECT_MARGIN);
void getLocalBounds(vec3& _min, vec3& _max) const override;
void setLocalScaling(const vec3& _scaling) override;
void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
void computeAABB(AABB& aabb, const etk::Transform3D& _transform) const override;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
// Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType);
/// Return the coordinates of a given vertex of the triangle
vec3 getVertex(int32_t index) const;
/**
* @brief Set the raycast test type (front, back, front-back)
* @param[in] _testType Raycast test type for the triangle (front, back, front-back)
*/
void setRaycastTestType(TriangleRaycastSide _testType);
/**
* @brief Return the coordinates of a given vertex of the triangle
* @param[in] _index Index (0 to 2) of a vertex of the triangle
*/
vec3 getVertex(int32_t _index) const;
friend class ConcaveMeshRaycastCallback;
friend class TriangleOverlapCallback;
};