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; using namespace ephysics;
// Constructor
ContactManifold::ContactManifold(ProxyShape* _shape1, ContactManifold::ContactManifold(ProxyShape* _shape1,
ProxyShape* _shape2, ProxyShape* _shape2,
short _normalDirectionId): short _normalDirectionId):
@ -25,21 +24,16 @@ ContactManifold::ContactManifold(ProxyShape* _shape1,
} }
// Destructor
ContactManifold::~ContactManifold() { ContactManifold::~ContactManifold() {
clear(); clear();
} }
// Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) { void ContactManifold::addContactPoint(ContactPoint* contact) {
// For contact already in the manifold // For contact already in the manifold
for (uint32_t i=0; i<m_nbContactPoints; i++) { for (uint32_t i=0; i<m_nbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point // Check if the new point point does not correspond to a same contact point
// already in the manifold. // already in the manifold.
float distance = (m_contactPoints[i]->getWorldPointOnBody1() - float distance = (m_contactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).length2();
contact->getWorldPointOnBody1()).length2();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact // Delete the new contact
ETK_DELETE(ContactPoint, contact); ETK_DELETE(ContactPoint, contact);
@ -47,26 +41,21 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
return; return;
} }
} }
// If the contact manifold is full // If the contact manifold is full
if (m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { if (m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact); int32_t indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); int32_t indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove); removeContactPoint(indexToRemove);
} }
// Add the new contact point in the manifold // Add the new contact point in the manifold
m_contactPoints[m_nbContactPoints] = contact; m_contactPoints[m_nbContactPoints] = contact;
m_nbContactPoints++; m_nbContactPoints++;
assert(m_nbContactPoints > 0); assert(m_nbContactPoints > 0);
} }
// Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint32_t index) { void ContactManifold::removeContactPoint(uint32_t index) {
assert(index < m_nbContactPoints); assert(index < m_nbContactPoints);
assert(m_nbContactPoints > 0); assert(m_nbContactPoints > 0);
// Call the destructor explicitly and tell the memory allocator that // Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free // the corresponding memory block is now free
ETK_DELETE(ContactPoint, m_contactPoints[index]); ETK_DELETE(ContactPoint, m_contactPoints[index]);
@ -75,156 +64,116 @@ void ContactManifold::removeContactPoint(uint32_t index) {
if (index < m_nbContactPoints - 1) { if (index < m_nbContactPoints - 1) {
m_contactPoints[index] = m_contactPoints[m_nbContactPoints - 1]; m_contactPoints[index] = m_contactPoints[m_nbContactPoints - 1];
} }
m_nbContactPoints--; 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) { void ContactManifold::update(const etk::Transform3D& transform1, const etk::Transform3D& transform2) {
if (m_nbContactPoints == 0) {
if (m_nbContactPoints == 0) return; return;
}
// Update the world coordinates and penetration depth of the contact points in the manifold // Update the world coordinates and penetration depth of the contact points in the manifold
for (uint32_t i=0; i<m_nbContactPoints; i++) { for (uint32_t i=0; i<m_nbContactPoints; i++) {
m_contactPoints[i]->setWorldPointOnBody1(transform1 * m_contactPoints[i]->setWorldPointOnBody1(transform1 * m_contactPoints[i]->getLocalPointOnBody1());
m_contactPoints[i]->getLocalPointOnBody1()); m_contactPoints[i]->setWorldPointOnBody2(transform2 * m_contactPoints[i]->getLocalPointOnBody2());
m_contactPoints[i]->setWorldPointOnBody2(transform2 * m_contactPoints[i]->setPenetrationDepth((m_contactPoints[i]->getWorldPointOnBody1() - m_contactPoints[i]->getWorldPointOnBody2()).dot(m_contactPoints[i]->getNormal()));
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 // 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--) { for (int32_t i=static_cast<int32_t>(m_nbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int32_t>(m_nbContactPoints)); assert(i < static_cast<int32_t>(m_nbContactPoints));
// Compute the distance between contact points in the normal direction // Compute the distance between contact points in the normal direction
float distanceNormal = -m_contactPoints[i]->getPenetrationDepth(); float distanceNormal = -m_contactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction // If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) { if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i); removeContactPoint(i);
} } else {
else {
// Compute the distance of the two contact points in the plane // Compute the distance of the two contact points in the plane
// orthogonal to the contact normal // orthogonal to the contact normal
vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() + vec3 projOfPoint1 = m_contactPoints[i]->getWorldPointOnBody1() + m_contactPoints[i]->getNormal() * distanceNormal;
m_contactPoints[i]->getNormal() * distanceNormal;
vec3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1; vec3 projDifference = m_contactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance // If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact // threshold, we remove the contact
if (projDifference.length2() > squarePersistentContactThreshold) { if (projDifference.length2() > squarePersistentContactThreshold) {
removeContactPoint(i); 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 { int32_t ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
int32_t indexMaxPenetrationDepth = -1; int32_t indexMaxPenetrationDepth = -1;
float maxPenetrationDepth = newContact->getPenetrationDepth(); float maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache // For each contact in the cache
for (uint32_t i=0; i<m_nbContactPoints; i++) { for (uint32_t i=0; i<m_nbContactPoints; i++) {
// If the current contact has a larger penetration depth // If the current contact has a larger penetration depth
if (m_contactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) { if (m_contactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = m_contactPoints[i]->getPenetrationDepth(); maxPenetrationDepth = m_contactPoints[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i; indexMaxPenetrationDepth = i;
} }
} }
// Return the index of largest penetration depth // Return the index of largest penetration depth
return indexMaxPenetrationDepth; 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 { int32_t ContactManifold::getIndexToRemove(int32_t indexMaxPenetration, const vec3& newPoint) const {
assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); assert(m_nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
float area0 = 0.0f; // Area with contact 1,2,3 and newPoint
float area0 = 0.0; // Area with contact 1,2,3 and newPoint float area1 = 0.0f; // Area with contact 0,2,3 and newPoint
float area1 = 0.0; // Area with contact 0,2,3 and newPoint float area2 = 0.0f; // Area with contact 0,1,3 and newPoint
float area2 = 0.0; // Area with contact 0,1,3 and newPoint float area3 = 0.0f; // Area with contact 0,1,2 and newPoint
float area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) { if (indexMaxPenetration != 0) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1(); vec3 vector1 = newPoint - m_contactPoints[1]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1();
m_contactPoints[2]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); vec3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.length2(); area0 = crossProduct.length2();
} }
if (indexMaxPenetration != 1) { if (indexMaxPenetration != 1) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[2]->getLocalPointOnBody1();
m_contactPoints[2]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); vec3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.length2(); area1 = crossProduct.length2();
} }
if (indexMaxPenetration != 2) { if (indexMaxPenetration != 2) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - vec3 vector2 = m_contactPoints[3]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1();
m_contactPoints[1]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); vec3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.length2(); area2 = crossProduct.length2();
} }
if (indexMaxPenetration != 3) { if (indexMaxPenetration != 3) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1(); vec3 vector1 = newPoint - m_contactPoints[0]->getLocalPointOnBody1();
vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() - vec3 vector2 = m_contactPoints[2]->getLocalPointOnBody1() - m_contactPoints[1]->getLocalPointOnBody1();
m_contactPoints[1]->getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); vec3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.length2(); area3 = crossProduct.length2();
} }
// Return the index of the contact to remove // Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3); 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 { int32_t ContactManifold::getMaxArea(float area0, float area1, float area2, float area3) const {
if (area0 < area1) { if (area0 < area1) {
if (area1 < area2) { if (area1 < area2) {
if (area2 < area3) return 3; if (area2 < area3) {
else return 2; return 3;
} else {
return 2;
}
} else {
if (area1 < area3) {
return 3;
} else {
return 1;
}
} }
else { } else {
if (area1 < area3) return 3;
else return 1;
}
}
else {
if (area0 < area2) { if (area0 < area2) {
if (area2 < area3) return 3; if (area2 < area3) return 3;
else return 2; else return 2;
} } else {
else {
if (area0 < area3) return 3; if (area0 < area3) return 3;
else return 0; else return 0;
} }

50
ephysics/collision/ContactManifold.hpp
View File

@ -49,6 +49,17 @@ namespace ephysics {
* The new added point is always kept. * The new added point is always kept.
*/ */
class ContactManifold { 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: private:
ProxyShape* m_shape1; //!< Pointer to the first proxy shape of the contact ProxyShape* m_shape1; //!< Pointer to the first proxy shape of the contact
ProxyShape* m_shape2; //!< Pointer to the second 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 float m_frictionTwistImpulse; //!< Twist friction constraint accumulated impulse
vec3 m_rollingResistanceImpulse; //!< Accumulated rolling resistance impulse vec3 m_rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
bool m_isAlreadyInIsland; //!< True if the contact manifold has already been added int32_to an island 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 /// Return the index of maximum area
int32_t getMaxArea(float _area0, float _area1, float _area2, float _area3) const; 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; 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; int32_t getIndexToRemove(int32_t _indexMaxPenetration, const vec3& _newPoint) const;
/// Remove a contact point from the manifold /// Remove a contact point from the manifold
void removeContactPoint(uint32_t _index); void removeContactPoint(uint32_t _index);
/// Return true if the contact manifold has already been added int32_to an island /// Return true if the contact manifold has already been added int32_to an island
bool isAlreadyInIsland() const; bool isAlreadyInIsland() const;
public: public:
/// Constructor
ContactManifold(ProxyShape* _shape1,
ProxyShape* _shape2,
int16_t _normalDirectionId);
/// Destructor
~ContactManifold();
/// Return a pointer to the first proxy shape of the contact /// Return a pointer to the first proxy shape of the contact
ProxyShape* getShape1() const; ProxyShape* getShape1() const;
/// Return a pointer to the second proxy shape of the contact /// Return a pointer to the second proxy shape of the contact
@ -95,7 +111,15 @@ namespace ephysics {
int16_t getNormalDirectionId() const; int16_t getNormalDirectionId() const;
/// Add a contact point to the manifold /// Add a contact point to the manifold
void addContactPoint(ContactPoint* _contact); 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, void update(const etk::Transform3D& _transform1,
const etk::Transform3D& _transform2); const etk::Transform3D& _transform2);
/// Clear the contact manifold /// 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; return supportPoint;
} }
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool ConeShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
const vec3 r = ray.point2 - ray.point1; const vec3 r = _ray.point2 - _ray.point1;
const float epsilon = float(0.00001); const float epsilon = float(0.00001);
vec3 V(0, m_halfHeight, 0); vec3 V(0, m_halfHeight, 0);
vec3 centerBase(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 heightSquare = float(4.0) * m_halfHeight * m_halfHeight;
float cosThetaSquare = heightSquare / (heightSquare + m_radius * m_radius); float cosThetaSquare = heightSquare / (heightSquare + m_radius * m_radius);
float factor = 1.0f - cosThetaSquare; 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 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 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(); 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 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; return false;
} }
localHitPoint[0] = ray.point1 + tHit[0] * r; localHitPoint[0] = _ray.point1 + tHit[0] * r;
localHitPoint[1] = ray.point1 + tHit[1] * 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) // 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) { if (axis.dot(localHitPoint[0] - V) < 0.0f) {
tHit[0] = float(-1.0); 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 the ray is in direction of the base plane of the cone
if (r.y() > epsilon) { if (r.y() > epsilon) {
// Compute the int32_tersection with the base plane of the cone // 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 // 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) { if ((localHitPoint[2] - centerBase).length2() > m_radius * m_radius) {
tHit[2] = float(-1.0); tHit[2] = float(-1.0);
} }
@ -131,7 +131,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
if (hitIndex < 0) { if (hitIndex < 0) {
return false; return false;
} }
if (t > ray.maxFraction) { if (t > _ray.maxFraction) {
return false; return false;
} }
// Compute the normal direction for hit against side of the cone // 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].setY(etk::sqrt(x * x + z * z) * rOverH);
localNormal[hitIndex].setZ(z); localNormal[hitIndex].setZ(z);
} }
raycastInfo.body = proxyShape->getBody(); _raycastInfo.body = _proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; _raycastInfo.proxyShape = _proxyShape;
raycastInfo.hitFraction = t; _raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint[hitIndex]; _raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex]; _raycastInfo.worldNormal = localNormal[hitIndex];
return true; return true;
} }
@ -163,40 +163,40 @@ float ConeShape::getHeight() const {
return float(2.0) * m_halfHeight; return float(2.0) * m_halfHeight;
} }
void ConeShape::setLocalScaling(const vec3& scaling) { void ConeShape::setLocalScaling(const vec3& _scaling) {
m_halfHeight = (m_halfHeight / m_scaling.y()) * scaling.y(); m_halfHeight = (m_halfHeight / m_scaling.y()) * _scaling.y();
m_radius = (m_radius / m_scaling.x()) * scaling.x(); m_radius = (m_radius / m_scaling.x()) * _scaling.x();
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(_scaling);
} }
size_t ConeShape::getSizeInBytes() const { size_t ConeShape::getSizeInBytes() const {
return sizeof(ConeShape); return sizeof(ConeShape);
} }
void ConeShape::getLocalBounds(vec3& min, vec3& max) const { void ConeShape::getLocalBounds(vec3& _min, vec3& _max) const {
// Maximum bounds // Maximum bounds
max.setX(m_radius + m_margin); _max.setX(m_radius + m_margin);
max.setY(m_halfHeight + m_margin); _max.setY(m_halfHeight + m_margin);
max.setZ(max.x()); _max.setZ(_max.x());
// Minimum bounds // Minimum bounds
min.setX(-max.x()); _min.setX(-_max.x());
min.setY(-max.y()); _min.setY(-_max.y());
min.setZ(min.x()); _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 rSquare = m_radius * m_radius;
float diagXZ = float(0.15) * mass * (rSquare + m_halfHeight); float diagXZ = float(0.15) * _mass * (rSquare + m_halfHeight);
tensor.setValue(diagXZ, 0.0, 0.0, _tensor.setValue(diagXZ, 0.0, 0.0,
0.0, float(0.3) * mass * rSquare, 0.0, float(0.3) * _mass * rSquare,
0.0, 0.0, 0.0, diagXZ); 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 const float radiusHeight = m_radius
* (-localPoint.y() + m_halfHeight) * (-_localPoint.y() + m_halfHeight)
/ (m_halfHeight * float(2.0)); / (m_halfHeight * float(2.0));
return ( localPoint.y() < m_halfHeight return ( _localPoint.y() < m_halfHeight
&& localPoint.y() > -m_halfHeight) && _localPoint.y() > -m_halfHeight)
&& (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); && (_localPoint.x() * _localPoint.x() + _localPoint.z() * _localPoint.z() < radiusHeight *radiusHeight);
} }