Merge pull request #1502 from AndrewMeadows/master

more correct particle-particle collisions

libraries/particles/src/Particle.cpp

@@ -58,6 +58,7 @@ void Particle::init(glm::vec3 position, float radius, rgbColor color, glm::vec3
 
     _position = position;
     _radius = radius;
+    _mass = 1.0f;
     memcpy(_color, color, sizeof(_color));
     _velocity = velocity;
     _damping = damping;
@@ -67,6 +68,11 @@ void Particle::init(glm::vec3 position, float radius, rgbColor color, glm::vec3
     _shouldDie = false;
 }
 
+void Particle::setMass(float value) {
+    if (value > 0.0f) {
+        _mass = value;
+    }
+}
 
 bool Particle::appendParticleData(OctreePacketData* packetData) const {
 

libraries/particles/src/Particle.h

@@ -69,6 +69,7 @@ public:
     const rgbColor& getColor() const { return _color; }
     xColor getXColor() const { xColor color = { _color[RED_INDEX], _color[GREEN_INDEX], _color[BLUE_INDEX] }; return color; }
     float getRadius() const { return _radius; }
+    float getMass() const { return _mass; }
     const glm::vec3& getVelocity() const { return _velocity; }
     const glm::vec3& getGravity() const { return _gravity; }
     bool getInHand() const { return _inHand; }
@@ -98,6 +99,7 @@ public:
             _color[BLUE_INDEX] = value.blue;
     }
     void setRadius(float value) { _radius = value; }
+    void setMass(float value); 
     void setGravity(const glm::vec3& value) { _gravity = value; }
     void setInHand(bool inHand) { _inHand = inHand; }
     void setDamping(float value) { _damping = value; }
@@ -149,6 +151,7 @@ protected:
     glm::vec3 _position;
     rgbColor _color;
     float _radius;
+    float _mass;
     glm::vec3 _velocity;
     uint32_t _id;
     static uint32_t _nextID;

libraries/particles/src/ParticleCollisionSystem.cpp

@@ -87,41 +87,50 @@ void ParticleCollisionSystem::updateCollisionWithVoxels(Particle* particle) {
     }
 }
 
-void ParticleCollisionSystem::updateCollisionWithParticles(Particle* particle) {
-    glm::vec3 center = particle->getPosition() * (float)(TREE_SCALE);
-    float radius = particle->getRadius() * (float)(TREE_SCALE);
-    const float ELASTICITY = 1.4f;
-    const float DAMPING = 0.0f;
+void ParticleCollisionSystem::updateCollisionWithParticles(Particle* particleA) {
+    glm::vec3 center = particleA->getPosition() * (float)(TREE_SCALE);
+    float radius = particleA->getRadius() * (float)(TREE_SCALE);
+    //const float ELASTICITY = 0.4f;
+    //const float DAMPING = 0.0f;
     const float COLLISION_FREQUENCY = 0.5f;
     glm::vec3 penetration;
-    Particle* penetratedParticle;
-    if (_particles->findSpherePenetration(center, radius, penetration, (void**)&penetratedParticle)) {
+    Particle* particleB;
+    if (_particles->findSpherePenetration(center, radius, penetration, (void**)&particleB)) {
+        // NOTE: 'penetration' is the depth that 'particleA' overlaps 'particleB'.
+        // That is, it points from A into B.
+    
+        // Even if the particles overlap... when the particles are already moving appart
+        // we don't want to count this as a collision.
+        glm::vec3 relativeVelocity = particleA->getVelocity() - particleB->getVelocity();
+        if (glm::dot(relativeVelocity, penetration) > 0.0f) {
+            particleA->collisionWithParticle(particleB);
+            particleB->collisionWithParticle(particleA);
 
-        // let the particles run their collision scripts if they have them
-        particle->collisionWithParticle(penetratedParticle);
-        penetratedParticle->collisionWithParticle(particle);
+            glm::vec3 axis = glm::normalize(penetration);
+            glm::vec3 axialVelocity = glm::dot(relativeVelocity, axis) * axis;
 
-        penetration /= (float)(TREE_SCALE);
-        updateCollisionSound(particle, penetration, COLLISION_FREQUENCY);
+            // particles that are in hand are assigned an ureasonably large mass for collisions
+            // which effectively makes them immovable but allows the other ball to reflect correctly.
+            const float MAX_MASS = 1.0e6f;
+            float massA = (particleA->getInHand()) ? MAX_MASS : particleA->getMass();
+            float massB = (particleB->getInHand()) ? MAX_MASS : particleB->getMass();
+            float totalMass = massA + massB;
+    
+            particleA->setVelocity(particleA->getVelocity() - axialVelocity * (2.0f * massB / totalMass));
 
-        // apply a hard collision to both particles of half the penetration each
+            ParticleEditHandle particleEditHandle(_packetSender, _particles, particleA->getID());
+            particleEditHandle.updateParticle(particleA->getPosition(), particleA->getRadius(), particleA->getXColor(), particleA->getVelocity(),
+                           particleA->getGravity(), particleA->getDamping(), particleA->getInHand(), particleA->getScript());
 
-        float particleShare, penetratedParticleShare;
-        if (particle->getInHand() && penetratedParticle->getInHand()) {
-            particleShare = 0.5f;
-            penetratedParticleShare = -0.5f;
-        } else if (particle->getInHand()) {
-            particleShare = 0.f;
-            penetratedParticleShare = -1.f;
-        } else if (penetratedParticle->getInHand()) {
-            particleShare = -1.f;
-            penetratedParticleShare = 0.f;
-        } else {
-            particleShare = 0.5f;
-            penetratedParticleShare = -0.5f;
+            particleB->setVelocity(particleB->getVelocity() + axialVelocity * (2.0f * massA / totalMass));
+
+            ParticleEditHandle penetratedparticleEditHandle(_packetSender, _particles, particleB->getID());
+            penetratedparticleEditHandle.updateParticle(particleB->getPosition(), particleB->getRadius(), particleB->getXColor(), particleB->getVelocity(),
+                           particleB->getGravity(), particleB->getDamping(), particleB->getInHand(), particleB->getScript());
+
+            penetration /= (float)(TREE_SCALE);
+            updateCollisionSound(particleA, penetration, COLLISION_FREQUENCY);
         }
-        applyHardCollision(particle, penetration * particleShare, ELASTICITY, DAMPING);
-        applyHardCollision(penetratedParticle, penetration * penetratedParticleShare, ELASTICITY, DAMPING);
     }
 }
 

libraries/particles/src/ParticleTree.h

@@ -12,11 +12,6 @@
 #include <Octree.h>
 #include "ParticleTreeElement.h"
 
-class ParticleTreeUpdateArgs {
-public:
-    std::vector<Particle> _movingParticles;
-};
-
 class NewlyCreatedParticleHook {
 public:
     virtual void particleCreated(const Particle& newParticle, Node* senderNode) = 0;

libraries/particles/src/ParticleTreeElement.h

@@ -19,7 +19,11 @@
 
 class ParticleTree;
 class ParticleTreeElement;
-class ParticleTreeUpdateArgs;
+
+class ParticleTreeUpdateArgs {
+public:
+    std::vector<Particle> _movingParticles;
+};
 
 class ParticleTreeElement : public OctreeElement {
     friend class ParticleTree; // to allow createElement to new us...
@@ -95,4 +99,4 @@ protected:
     std::vector<Particle> _particles;
 };
 
-#endif /* defined(__hifi__ParticleTreeElement__) */
+#endif /* defined(__hifi__ParticleTreeElement__) */

libraries/shared/src/GeometryUtil.cpp

@@ -31,105 +31,107 @@ glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec
     }
 }
 
-bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction,
-                           float combinedRadius, glm::vec3& penetration) {
-    float vectorLength = glm::length(penetratorToPenetratee);
+// Computes the penetration between a point and a sphere (centered at the origin)
+// if point is inside sphere: returns true and stores the result in 'penetration' 
+// (the vector that would move the point outside the sphere)
+// otherwise returns false
+bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, float sphereRadius, 
+                           glm::vec3& penetration) {
+    float vectorLength = glm::length(point);
     if (vectorLength < EPSILON) {
-        penetration = direction * combinedRadius;
+        penetration = defaultDirection * sphereRadius;
         return true;
     }
-    float distance = vectorLength - combinedRadius;
+    float distance = vectorLength - sphereRadius;
     if (distance < 0.0f) {
-        penetration = penetratorToPenetratee * (-distance / vectorLength);
+        penetration = point * (-distance / vectorLength);
         return true;
     }
     return false;
 }
 
-bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-                                const glm::vec3& penetrateeLocation, glm::vec3& penetration) {
-    return findSpherePenetration(penetrateeLocation - penetratorCenter, glm::vec3(0.0f, -1.0f, 0.0f),
-        penetratorRadius, penetration);
+bool findSpherePointPenetration(const glm::vec3& sphereCenter, float sphereRadius,
+                                const glm::vec3& point, glm::vec3& penetration) {
+    return findSpherePenetration(point - sphereCenter, glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);
 }
 
-bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter,
-       float penetrateeRadius, glm::vec3& penetration) {
-    return findSpherePenetration(penetrateeCenter - penetratorLocation, glm::vec3(0.0f, -1.0f, 0.0f),
-        penetrateeRadius, penetration);
+bool findPointSpherePenetration(const glm::vec3& point, const glm::vec3& sphereCenter,
+       float sphereRadius, glm::vec3& penetration) {
+    return findSpherePenetration(sphereCenter - point, glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);
 }
 
-bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-                                 const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
-    return findSpherePointPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, penetrateeCenter, penetration);
+bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius,
+                                 const glm::vec3& secondCenter, float secondRadius, glm::vec3& penetration) {
+    return findSpherePointPenetration(firstCenter, firstRadius + secondRadius, secondCenter, penetration);
 }
 
-bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-                                  const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration) {
-    return findSpherePenetration(computeVectorFromPointToSegment(penetratorCenter, penetrateeStart, penetrateeEnd),
-        glm::vec3(0.0f, -1.0f, 0.0f), penetratorRadius, penetration);
+bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius,
+                                  const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration) {
+    return findSpherePenetration(computeVectorFromPointToSegment(sphereCenter, segmentStart, segmentEnd), 
+                                 glm::vec3(0.0f, -1.0f, 0.0f), sphereRadius, penetration);
 }
 
-bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart,
-                                  const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration) {
-    return findSphereSegmentPenetration(penetratorCenter, penetratorRadius + penetrateeRadius,
-        penetrateeStart, penetrateeEnd, penetration);
+bool findSphereCapsulePenetration(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& capsuleStart,
+                                  const glm::vec3& capsuleEnd, float capsuleRadius, glm::vec3& penetration) {
+    return findSphereSegmentPenetration(sphereCenter, sphereRadius + capsuleRadius,
+        capsuleStart, capsuleEnd, penetration);
 }
 
-bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart,
-        const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) {
+bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& capsuleStart,
+        const glm::vec3& capsuleEnd, float startRadius, float endRadius, glm::vec3& penetration) {
     // compute the projection of the point vector onto the segment vector
-    glm::vec3 segmentVector = penetrateeEnd - penetrateeStart;
+    glm::vec3 segmentVector = capsuleEnd - capsuleStart;
     float lengthSquared = glm::dot(segmentVector, segmentVector);
     if (lengthSquared < EPSILON) { // start and end the same
-        return findPointSpherePenetration(penetratorLocation, penetrateeStart,
-            glm::max(penetrateeStartRadius, penetrateeEndRadius), penetration);
+        return findPointSpherePenetration(point, capsuleStart,
+            glm::max(startRadius, endRadius), penetration);
     }
-    float proj = glm::dot(penetratorLocation - penetrateeStart, segmentVector) / lengthSquared;
+    float proj = glm::dot(point - capsuleStart, segmentVector) / lengthSquared;
     if (proj <= 0.0f) { // closest to the start
-        return findPointSpherePenetration(penetratorLocation, penetrateeStart, penetrateeStartRadius, penetration);
+        return findPointSpherePenetration(point, capsuleStart, startRadius, penetration);
         
     } else if (proj >= 1.0f) { // closest to the end
-        return findPointSpherePenetration(penetratorLocation, penetrateeEnd, penetrateeEndRadius, penetration);
+        return findPointSpherePenetration(point, capsuleEnd, endRadius, penetration);
     
     } else { // closest to the middle
-        return findPointSpherePenetration(penetratorLocation, penetrateeStart + segmentVector * proj,
-            glm::mix(penetrateeStartRadius, penetrateeEndRadius, proj), penetration);
+        return findPointSpherePenetration(point, capsuleStart + segmentVector * proj,
+            glm::mix(startRadius, endRadius, proj), penetration);
     }
 }
 
-bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter,
-        float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd,
-        float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) {
-    return findPointCapsuleConePenetration(penetratorCenter, penetrateeStart, penetrateeEnd,
-        penetrateeStartRadius + penetratorRadius, penetrateeEndRadius + penetratorRadius, penetration);
+bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter,
+        float sphereRadius, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd,
+        float startRadius, float endRadius, glm::vec3& penetration) {
+    return findPointCapsuleConePenetration(sphereCenter, capsuleStart, capsuleEnd,
+        startRadius + sphereRadius, endRadius + sphereRadius, penetration);
 }
 
-bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
-                                const glm::vec4& penetrateePlane, glm::vec3& penetration) {
-    float distance = glm::dot(penetrateePlane, glm::vec4(penetratorCenter, 1.0f)) - penetratorRadius;
+bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+                                const glm::vec4& plane, glm::vec3& penetration) {
+    float distance = glm::dot(plane, glm::vec4(sphereCenter, 1.0f)) - sphereRadius;
     if (distance < 0.0f) {
-        penetration = glm::vec3(penetrateePlane) * distance;
+        penetration = glm::vec3(plane) * distance;
         return true;
     }
     return false;
 }
 
-bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
-                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
-    if (findSphereCapsulePenetration(penetrateeCenter, penetrateeRadius,
-            penetratorStart, penetratorEnd, penetratorRadius, penetration)) {
+bool findCapsuleSpherePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,
+                                  const glm::vec3& sphereCenter, float sphereRadius, glm::vec3& penetration) {
+    if (findSphereCapsulePenetration(sphereCenter, sphereRadius,
+            capsuleStart, capsuleEnd, capsuleRadius, penetration)) {
         penetration = -penetration;
         return true;
     }
     return false;
 }
 
-bool findCapsulePlanePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
-                                 const glm::vec4& penetrateePlane, glm::vec3& penetration) {
-    float distance = glm::min(glm::dot(penetrateePlane, glm::vec4(penetratorStart, 1.0f)),
-        glm::dot(penetrateePlane, glm::vec4(penetratorEnd, 1.0f))) - penetratorRadius;
+bool findCapsulePlanePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,
+                                 const glm::vec4& plane, glm::vec3& penetration) {
+    float distance = glm::min(glm::dot(plane, glm::vec4(capsuleStart, 1.0f)),
+        glm::dot(plane, glm::vec4(capsuleEnd, 1.0f))) - capsuleRadius;
     if (distance < 0.0f) {
-        penetration = glm::vec3(penetrateePlane) * distance;
+        penetration = glm::vec3(plane) * distance;
         return true;
     }
     return false;

libraries/shared/src/GeometryUtil.h

@@ -13,39 +13,45 @@
 
 glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end);
 
-bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::vec3& direction,
-                           float combinedRadius, glm::vec3& penetration);
+/// Computes the penetration between a point and a sphere (centered at the origin)
+/// \param point the point location relative to sphere center (origin)
+/// \param defaultDirection the direction of the pentration when the point is near the origin
+/// \param sphereRadius the radius of the sphere
+/// \param penetration the displacement that would move the point out of penetration with the sphere
+/// \return true if point is inside sphere, otherwise false
+bool findSpherePenetration(const glm::vec3& point, const glm::vec3& defaultDirection, 
+                           float sphereRadius, glm::vec3& penetration);
 
-bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-                                const glm::vec3& penetrateeLocation, glm::vec3& penetration);
+bool findSpherePointPenetration(const glm::vec3& sphereCenter, float sphereRadius,
+                                const glm::vec3& point, glm::vec3& penetration);
 
-bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter,
-    float penetrateeRadius, glm::vec3& penetration);
+bool findPointSpherePenetration(const glm::vec3& point, const glm::vec3& sphereCenter,
+    float sphereRadius, glm::vec3& penetration);
 
-bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-                                 const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);
+bool findSphereSpherePenetration(const glm::vec3& firstCenter, float firstRadius,
+                                 const glm::vec3& secondCenter, float secondRadius, glm::vec3& penetration);
                      
-bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-                                  const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd, glm::vec3& penetration);
+bool findSphereSegmentPenetration(const glm::vec3& sphereCenter, float sphereRadius,
+                                  const glm::vec3& segmentStart, const glm::vec3& segmentEnd, glm::vec3& penetration);
 
-bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart,
-                                  const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration);
+bool findSphereCapsulePenetration(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& capsuleStart,
+                                  const glm::vec3& capsuleEnd, float capsuleRadius, glm::vec3& penetration);
 
-bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart,
-    const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration);
+bool findPointCapsuleConePenetration(const glm::vec3& point, const glm::vec3& capsuleStart,
+    const glm::vec3& capsuleEnd, float startRadius, float endRadius, glm::vec3& penetration);
     
-bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter,
-    float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd,
-    float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration);
+bool findSphereCapsuleConePenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+    const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd,
+    float startRadius, float endRadius, glm::vec3& penetration);
                                   
-bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
-                                const glm::vec4& penetrateePlane, glm::vec3& penetration);
+bool findSpherePlanePenetration(const glm::vec3& sphereCenter, float sphereRadius, 
+                                const glm::vec4& plane, glm::vec3& penetration);
                                   
-bool findCapsuleSpherePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
-                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);
+bool findCapsuleSpherePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,
+                                  const glm::vec3& sphereCenter, float sphereRadius, glm::vec3& penetration);
 
-bool findCapsulePlanePenetration(const glm::vec3& penetratorStart, const glm::vec3& penetratorEnd, float penetratorRadius,
-                                 const glm::vec4& penetrateePlane, glm::vec3& penetration);
+bool findCapsulePlanePenetration(const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius,
+                                 const glm::vec4& plane, glm::vec3& penetration);
 
 glm::vec3 addPenetrations(const glm::vec3& currentPenetration, const glm::vec3& newPenetration);