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-05-18 22:17:49 +02:00
parent b1fe6eebb4
commit 696a979395
3 changed files with 104 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
ephysics/collision/ContactManifold.cpp
View File

@ -10,7 +10,6 @@
using namespace ephysics;
// Constructor
ContactManifold::ContactManifold(ProxyShape* _shape1,
ProxyShape* _shape2,
short _normalDirectionId):
@ -25,21 +24,16 @@ ContactManifold::ContactManifold(ProxyShape* _shape1,
}
// Destructor
ContactManifold::~ContactManifold() {
clear();
}
// Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) {
// For contact already in the manifold
for (uint32_t i=0; i<m_nbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point
// already in the manifold.
float distance = (m_contactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).length2();
float distance = (m_contactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).length2();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact
ETK_DELETE(ContactPoint, contact);
@ -47,26 +41,21 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
return;
}
}
// If the contact manifold is full
if (m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove);
}
// Add the new contact point in the manifold
m_contactPoints[m_nbContactPoints] = contact;
m_nbContactPoints++;
assert(m_nbContactPoints > 0);
}
// Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint32_t index) {
assert(index < m_nbContactPoints);
assert(m_nbContactPoints > 0);
// Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free
ETK_DELETE(ContactPoint, m_contactPoints[index]);
@ -75,156 +64,116 @@ void ContactManifold::removeContactPoint(uint32_t index) {
if (index < m_nbContactPoints - 1) {
m_contactPoints[index] = m_contactPoints[m_nbContactPoints - 1];
}
m_nbContactPoints--;
}
// Update the contact manifold
/// First the world space coordinates of the current contacts in the manifold are recomputed from
/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
/// the contacts with a too large distance between the contact points in the plane orthogonal to the
/// contact normal.
void ContactManifold::update(const etk::Transform3D& transform1, const etk::Transform3D& transform2) {
if (m_nbContactPoints == 0) return;
if (m_nbContactPoints == 0) {
return;
}
// Update the world coordinates and penetration depth of the contact points in the manifold
for (uint32_t i=0; i<m_nbContactPoints; i++) {
m_contactPoints[i]->setWorldPointOnBody1(transform1 *
m_contactPoints[i]->getLocalPointOnBody1());
m_contactPoints[i]->setWorldPointOnBody2(transform2 *
m_contactPoints[i]->getLocalPointOnBody2());
m_contactPoints[i]->setPenetrationDepth((m_contactPoints[i]->getWorldPointOnBody1() -
m_contactPoints[i]->getWorldPointOnBody2()).dot(m_contactPoints[i]->getNormal()));
m_contactPoints[i]->setWorldPointOnBody1(transform1 * m_contactPoints[i]->getLocalPointOnBody1());
m_contactPoints[i]->setWorldPointOnBody2(transform2 * m_contactPoints[i]->getLocalPointOnBody2());
m_contactPoints[i]->setPenetrationDepth((m_contactPoints[i]->getWorldPointOnBody1() - m_contactPoints[i]->getWorldPointOnBody2()).dot(m_contactPoints[i]->getNormal()));
}
const float squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD;
const float squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD;
// Remove the contact points that don't represent very well the contact manifold
for (int32_t i=static_cast<int32_t>(m_nbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int32_t>(m_nbContactPoints));
// Compute the distance between contact points in the normal direction
float distanceNormal = -m_contactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i);
}
else {
} else {
// Compute the distance of the two contact points in the plane
// orthogonal to the contact normal
vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() +
m_contactPoints[i]->getNormal() * distanceNormal;
vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() + m_contactPoints[i]->getNormal() * distanceNormal;
vec3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact
if (projDifference.length2() > squarePersistentContactThreshold) {
removeContactPoint(i);
}
}
}
}
}
// Return the index of the contact point with the larger penetration depth.
/// This corresponding contact will be kept in the cache. The method returns -1 is
/// the new contact is the deepest.
int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
int32_t indexMaxPenetrationDepth = -1;
float maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache
for (uint32_t i=0; i<m_nbContactPoints; i++) {
// If the current contact has a larger penetration depth
if (m_contactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = m_contactPoints[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i;
}
}
// Return the index of largest penetration depth
return indexMaxPenetrationDepth;
}
// Return the index that will be removed.
/// The index of the contact point with the larger penetration
/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
/// the different area and we want to keep the contacts with the largest area. The new point is also
/// kept. In order to compute the area of a quadrilateral, we use the formula :
/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
/// when we compute this area, we do not calculate it exactly but we
/// 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 ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const vec3& newPoint) const {
assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
float area0 = 0.0; // Area with contact 1,2,3 and newPoint
float area1 = 0.0; // Area with contact 0,2,3 and newPoint
float area2 = 0.0; // Area with contact 0,1,3 and newPoint
float area3 = 0.0; // Area with contact 0,1,2 and newPoint
float area0 = 0.0f; // Area with contact 1,2,3 and newPoint
float area1 = 0.0f; // Area with contact 0,2,3 and newPoint
float area2 = 0.0f; // Area with contact 0,1,3 and newPoint
float area3 = 0.0f; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
vec3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
m_contactPoints[2]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.length2();
}
if (indexMaxPenetration != 1) {
// Compute the area
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
m_contactPoints[2]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.length2();
}
if (indexMaxPenetration != 2) {
// Compute the area
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() -
m_contactPoints[1]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.length2();
}
if (indexMaxPenetration != 3) {
// Compute the area
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() -
m_contactPoints[1]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.length2();
}
// Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3);
}
// Return the index of maximum area
int32_t ContactManifold::getMaxArea(float area0, float area1, float area2, float area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;
else return 2;
if (area2 < area3) {
return 3;
} else {
return 2;
}
} else {
if (area1 < area3) {
return 3;
} else {
return 1;
}
}
else {
if (area1 < area3) return 3;
else return 1;
}
}
else {
} else {
if (area0 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
} else {
if (area0 < area3) return 3;
else return 0;
}

50
ephysics/collision/ContactManifold.hpp
View File

@ -49,6 +49,17 @@ namespace ephysics {
* The new added point is always kept.
*/
class ContactManifold {
public:
/// Constructor
ContactManifold(ProxyShape* _shape1,
ProxyShape* _shape2,
int16_t _normalDirectionId);
/// Destructor
~ContactManifold();
/// DELETE copy-constructor
ContactManifold(const ContactManifold& _contactManifold) = delete;
/// DELETE assignment operator
ContactManifold& operator=(const ContactManifold& _contactManifold) = delete;
private:
ProxyShape* m_shape1; //!< Pointer to the first proxy shape of the contact
ProxyShape* m_shape2; //!< Pointer to the second proxy shape of the contact
@ -62,27 +73,32 @@ namespace ephysics {
float m_frictionTwistImpulse; //!< Twist friction constraint accumulated impulse
vec3 m_rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
bool m_isAlreadyInIsland; //!< True if the contact manifold has already been added int32_to an island
/// Private copy-constructor
ContactManifold(const ContactManifold& _contactManifold) = delete;
/// Private assignment operator
ContactManifold& operator=(const ContactManifold& _contactManifold) = delete;
/// Return the index of maximum area
int32_t getMaxArea(float _area0, float _area1, float _area2, float _area3) const;
/// Return the index of the contact with the larger penetration depth.
/**
* @brief Return the index of the contact with the larger penetration depth.
*
* This corresponding contact will be kept in the cache. The method returns -1 is
* the new contact is the deepest.
*/
int32_t getIndexOfDeepestPenetration(ContactPoint* _newContact) const;
/// Return the index that will be removed.
/**
* @brief Return the index that will be removed.
* The index of the contact point with the larger penetration
* depth is given as a parameter. This contact won't be removed. Given this contact, we compute
* the different area and we want to keep the contacts with the largest area. The new point is also
* kept. In order to compute the area of a quadrilateral, we use the formula :
* Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
* when we compute this area, we do not calculate it exactly but we
* 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);
/// Return true if the contact manifold has already been added int32_to an island
bool isAlreadyInIsland() const;
public:
/// Constructor
ContactManifold(ProxyShape* _shape1,
ProxyShape* _shape2,
int16_t _normalDirectionId);
/// Destructor
~ContactManifold();
/// Return a pointer to the first proxy shape of the contact
ProxyShape* getShape1() const;
/// Return a pointer to the second proxy shape of the contact
@ -95,7 +111,15 @@ namespace ephysics {
int16_t getNormalDirectionId() const;
/// Add a contact point to the manifold
void addContactPoint(ContactPoint* _contact);
/// Update the contact manifold.
/**
* @brief Update the contact manifold.
*
* First the world space coordinates of the current contacts in the manifold are recomputed from
* the corresponding transforms of the bodies because they have moved. Then we remove the contacts
* with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
* the contacts with a too large distance between the contact points in the plane orthogonal to the
* contact normal.
*/
void update(const etk::Transform3D& _transform1,
const etk::Transform3D& _transform2);
/// Clear the contact manifold

70
ephysics/collision/shapes/ConeShape.cpp
View File

@ -40,8 +40,8 @@ vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void**
return supportPoint;
}
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const vec3 r = ray.point2 - ray.point1;
bool ConeShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
const vec3 r = _ray.point2 - _ray.point1;
const float epsilon = float(0.00001);
vec3 V(0, m_halfHeight, 0);
vec3 centerBase(0, -m_halfHeight, 0);
@ -49,7 +49,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
float heightSquare = float(4.0) * m_halfHeight * m_halfHeight;
float cosThetaSquare = heightSquare / (heightSquare + m_radius * m_radius);
float factor = 1.0f - cosThetaSquare;
vec3 delta = ray.point1 - V;
vec3 delta = _ray.point1 - V;
float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.z();
float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z();
float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z();
@ -85,11 +85,11 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
}
}
// If the origin of the ray is inside the cone, we return no hit
if (testPointInside(ray.point1, NULL)) {
if (testPointInside(_ray.point1, NULL)) {
return false;
}
localHitPoint[0] = ray.point1 + tHit[0] * r;
localHitPoint[1] = ray.point1 + tHit[1] * r;
localHitPoint[0] = _ray.point1 + tHit[0] * r;
localHitPoint[1] = _ray.point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are int32_terested in)
if (axis.dot(localHitPoint[0] - V) < 0.0f) {
tHit[0] = float(-1.0);
@ -107,9 +107,9 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
// If the ray is in direction of the base plane of the cone
if (r.y() > epsilon) {
// Compute the int32_tersection with the base plane of the cone
tHit[2] = (-ray.point1.y() - m_halfHeight) / (r.y());
tHit[2] = (-_ray.point1.y() - m_halfHeight) / (r.y());
// Only keep this int32_tersection if it is inside the cone radius
localHitPoint[2] = ray.point1 + tHit[2] * r;
localHitPoint[2] = _ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).length2() > m_radius * m_radius) {
tHit[2] = float(-1.0);
}
@ -131,7 +131,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
if (hitIndex < 0) {
return false;
}
if (t > ray.maxFraction) {
if (t > _ray.maxFraction) {
return false;
}
// Compute the normal direction for hit against side of the cone
@ -147,11 +147,11 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
localNormal[hitIndex].setY(etk::sqrt(x * x + z * z) * rOverH);
localNormal[hitIndex].setZ(z);
}
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex];
_raycastInfo.body = _proxyShape->getBody();
_raycastInfo.proxyShape = _proxyShape;
_raycastInfo.hitFraction = t;
_raycastInfo.worldPoint = localHitPoint[hitIndex];
_raycastInfo.worldNormal = localNormal[hitIndex];
return true;
}
@ -163,40 +163,40 @@ float ConeShape::getHeight() const {
return float(2.0) * m_halfHeight;
}
void ConeShape::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);
void ConeShape::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);
}
size_t ConeShape::getSizeInBytes() const {
return sizeof(ConeShape);
}
void ConeShape::getLocalBounds(vec3& min, vec3& max) const {
void ConeShape::getLocalBounds(vec3& _min, vec3& _max) const {
// Maximum bounds
max.setX(m_radius + 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());
}
void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const {
float rSquare = m_radius * m_radius;
float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight);
tensor.setValue(diagXZ, 0.0, 0.0,
0.0, float(0.3) * mass * rSquare,
0.0, 0.0, 0.0, diagXZ);
float diagXZ = float(0.15) * _mass * (rSquare + m_halfHeight);
_tensor.setValue(diagXZ, 0.0, 0.0,
0.0, float(0.3) * _mass * rSquare,
0.0, 0.0, 0.0, diagXZ);
}
bool ConeShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const {
bool ConeShape::testPointInside(const vec3& _localPoint, ProxyShape* _proxyShape) const {
const float radiusHeight = m_radius
* (-localPoint.y() + m_halfHeight)
* (-_localPoint.y() + m_halfHeight)
/ (m_halfHeight * float(2.0));
return ( localPoint.y() < m_halfHeight
&& localPoint.y() > -m_halfHeight)
&& (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
return ( _localPoint.y() < m_halfHeight
&& _localPoint.y() > -m_halfHeight)
&& (_localPoint.x() * _localPoint.x() + _localPoint.z() * _localPoint.z() < radiusHeight *radiusHeight);
}