better implementation of particle palm collisions

interface/src/Application.cpp

@@ -1858,7 +1858,7 @@ void Application::init() {
     _particles.init();
     _particles.setViewFrustum(getViewFrustum());
     
-    _particleCollisionSystem.init(&_particleEditSender, _particles.getTree(), _voxels.getTree(), &_audio);
+    _particleCollisionSystem.init(&_particleEditSender, _particles.getTree(), _voxels.getTree(), &_audio, &_myAvatar);
     
     _palette.init(_glWidget->width(), _glWidget->height());
     _palette.addAction(Menu::getInstance()->getActionForOption(MenuOption::VoxelAddMode), 0, 0);

interface/src/avatar/Hand.cpp

@@ -85,7 +85,8 @@ void Hand::simulateToyBall(PalmData& palm, const glm::vec3& fingerTipPosition, f
         //printf("potentially caught... particle ID:%d\n", closestParticle->getID());
         
         if (!_toyBallInHand[handID]) {
-            printf("particle ID:%d NOT IN HAND\n", closestParticle->getID());
+            /***
+            printf("particle ID:%d IN TARGET AND NOT IN HAND\n", closestParticle->getID());
 
             // you can create a ParticleEditHandle by doing this...
             ParticleEditHandle* caughtParticle = Application::getInstance()->newParticleEditHandle(closestParticle->getID());
@@ -93,22 +94,22 @@ void Hand::simulateToyBall(PalmData& palm, const glm::vec3& fingerTipPosition, f
             // reflect off the hand...
             printf("particle ID:%d old velocity=%f,%f,%f\n", closestParticle->getID(), 
                     closestParticle->getVelocity().x, closestParticle->getVelocity().y, closestParticle->getVelocity().z);
-            glm::vec3 newVelocity = glm::reflect(closestParticle->getVelocity(), palm.getNormal());
+                    
+            glm::vec3 reflectNormal = glm::vec3(0,1,0); // palm.getNormal()                    
+            glm::vec3 newVelocity = glm::reflect(closestParticle->getVelocity(), reflectNormal);
 
             printf("particle ID:%d REFLECT velocity=%f,%f,%f\n", closestParticle->getID(), 
                     newVelocity.x, newVelocity.y, newVelocity.z);
 
-            newVelocity += palm.getTipVelocity() / (float)TREE_SCALE;
-
-            printf("particle ID:%d with TIP velocity=%f,%f,%f\n", closestParticle->getID(), 
-                    newVelocity.x, newVelocity.y, newVelocity.z);
+            //newVelocity += palm.getTipVelocity() / (float)TREE_SCALE;
+            //printf("particle ID:%d with TIP velocity=%f,%f,%f\n", closestParticle->getID(), newVelocity.x, newVelocity.y, newVelocity.z);
 
             
             printf("particle ID:%d OLD position=%f,%f,%f\n", closestParticle->getID(), 
                     closestParticle->getPosition().x, closestParticle->getPosition().y, closestParticle->getPosition().z);
             glm::vec3 newPosition = closestParticle->getPosition();
 
-            newPosition += newVelocity; // move it as if it's already been moving in new direction
+            newPosition += newVelocity * 10.f; // move it as if it's already been moving in new direction
 
             printf("particle ID:%d NEW position=%f,%f,%f\n", closestParticle->getID(), 
                     newPosition.x, newPosition.y, newPosition.z);
@@ -123,6 +124,7 @@ void Hand::simulateToyBall(PalmData& palm, const glm::vec3& fingerTipPosition, f
         
             // but make sure you clean it up, when you're done
             delete caughtParticle;
+            **/
         }
     }
     
@@ -496,6 +498,10 @@ void Hand::renderLeapHands() {
             }
             glTranslatef(targetPosition.x, targetPosition.y, targetPosition.z);
             glutWireSphere(targetRadius, 20.0f, 20.0f);
+            
+            const float collisionRadius = 0.05f;
+            glColor4f(0.5f,0.5f,0.5f, alpha);
+            glutWireSphere(collisionRadius, 20.0f, 20.0f);
             glPopMatrix();
         }
     }

interface/src/avatar/Hand.h

@@ -64,7 +64,7 @@ public:
     // Get the drag distance to move
     glm::vec3 getAndResetGrabDelta();
     glm::vec3 getAndResetGrabDeltaVelocity();
-    
+
 private:
     // disallow copies of the Hand, copy of owning Avatar is disallowed too
     Hand(const Hand&);

interface/src/avatar/Head.cpp

@@ -765,3 +765,4 @@ void Head::updateHairPhysics(float deltaTime) {
         _hairTuft[t].update(deltaTime);     
     }
 }
+

libraries/avatars/src/AvatarData.h

@@ -98,6 +98,9 @@ public:
 
     const QUuid& getLeaderUUID() const { return _leaderUUID; }
     
+    const HeadData* getHeadData() const { return _headData; }
+    const HandData* getHandData() const { return _handData; }
+
     void setHeadData(HeadData* headData) { _headData = headData; }
     void setHandData(HandData* handData) { _handData = handData; }
     

libraries/avatars/src/HandData.cpp

@@ -9,6 +9,7 @@
 #include "HandData.h"
 #include "AvatarData.h"
 #include <SharedUtil.h>
+#include <GeometryUtil.h>
 
 
 // When converting between fixed and float, use this as the radix.
@@ -223,6 +224,25 @@ void HandData::updateFingerTrails() {
     }
 }
 
+bool HandData::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, 
+                                        const PalmData*& collidingPalm) const {
+    
+    for (size_t i = 0; i < _palms.size(); ++i) {
+        const PalmData& palm = _palms[i];
+        if (!palm.isActive()) {
+            continue;
+        }
+        glm::vec3 palmPosition = palm.getPosition();
+        const float PALM_RADIUS = 0.05f; // in world (not voxel) coordinates
+        if (findSphereSpherePenetration(penetratorCenter, penetratorRadius, palmPosition, PALM_RADIUS, penetration)) {
+            collidingPalm = &palm;
+            return true;
+        }
+    }
+    return false;
+}
+
+
 void FingerData::setTrailLength(unsigned int length) {
     _tipTrailPositions.resize(length);
     _tipTrailCurrentStartIndex = 0;
@@ -265,3 +285,4 @@ const glm::vec3& FingerData::getTrailPosition(int index) {
 
 
 
+

libraries/avatars/src/HandData.h

@@ -68,6 +68,15 @@ public:
     int encodeRemoteData(unsigned char* destinationBuffer);
     int decodeRemoteData(unsigned char* sourceBuffer);
 
+    /// Checks for penetration between the described sphere and the hand.
+    /// \param penetratorCenter the center of the penetration test sphere
+    /// \param penetratorRadius the radius of the penetration test sphere
+    /// \param penetration[out] the vector in which to store the penetration
+    /// \param collidingPalm[out] a const PalmData* to the palm that was collided with
+    /// \return whether or not the sphere penetrated
+    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, 
+        const PalmData*& collidingPalm) const;
+
     friend class AvatarData;
 protected:
     glm::vec3              _basePosition;      // Hands are placed relative to this
@@ -124,6 +133,7 @@ public:
     PalmData(HandData* owningHandData);
     glm::vec3 getPosition() const { return _owningHandData->leapPositionToWorldPosition(_rawPosition); }
     glm::vec3 getNormal() const { return _owningHandData->leapDirectionToWorldDirection(_rawNormal); }
+    glm::vec3 getVelocity() const { return _owningHandData->leapDirectionToWorldDirection(_rawVelocity); }
 
     const glm::vec3& getRawPosition() const { return _rawPosition; }
     const glm::vec3& getRawNormal()   const { return _rawNormal; }

libraries/avatars/src/HeadData.cpp

@@ -44,3 +44,11 @@ void HeadData::addLean(float sideways, float forwards) {
     _leanSideways += sideways;
     _leanForward  += forwards;
 }
+
+bool HeadData::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration) const {
+    // we would like to update this to determine collisions/penetrations with the Avatar's head sphere...
+    // but right now it does not appear as if the HeadData has a position and radius.
+    // this is a placeholder for now.
+    return false;
+}
+

libraries/avatars/src/HeadData.h

@@ -58,6 +58,13 @@ public:
     void setLookAtPosition(const glm::vec3& lookAtPosition) { _lookAtPosition = lookAtPosition; }
     
     friend class AvatarData;
+
+    /// Checks for penetration between the described sphere and the hand.
+    /// \param penetratorCenter the center of the penetration test sphere
+    /// \param penetratorRadius the radius of the penetration test sphere
+    /// \param penetration[out] the vector in which to store the penetration
+    /// \return whether or not the sphere penetrated
+    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration) const;
     
 protected:
     float _yaw;

libraries/particles/src/ParticleCollisionSystem.cpp

@@ -9,6 +9,9 @@
 
 #include <AbstractAudioInterface.h>
 #include <VoxelTree.h>
+#include <AvatarData.h>
+#include <HeadData.h>
+#include <HandData.h>
 
 #include "Particle.h"
 #include "ParticleCollisionSystem.h"
@@ -17,16 +20,17 @@
 #include "ParticleTree.h"
 
 ParticleCollisionSystem::ParticleCollisionSystem(ParticleEditPacketSender* packetSender, 
-    ParticleTree* particles, VoxelTree* voxels, AbstractAudioInterface* audio) {
-    init(packetSender, particles, voxels, audio);
+    ParticleTree* particles, VoxelTree* voxels, AbstractAudioInterface* audio, AvatarData* selfAvatar) {
+    init(packetSender, particles, voxels, audio, selfAvatar);
 }
 
 void ParticleCollisionSystem::init(ParticleEditPacketSender* packetSender, 
-    ParticleTree* particles, VoxelTree* voxels, AbstractAudioInterface* audio) {
+    ParticleTree* particles, VoxelTree* voxels, AbstractAudioInterface* audio, AvatarData* selfAvatar) {
     _packetSender = packetSender;
     _particles = particles;
     _voxels = voxels;
     _audio = audio;
+    _selfAvatar = selfAvatar;
 }
 
 ParticleCollisionSystem::~ParticleCollisionSystem() {
@@ -59,6 +63,7 @@ void ParticleCollisionSystem::update() {
 void ParticleCollisionSystem::checkParticle(Particle* particle) {
     updateCollisionWithVoxels(particle);
     updateCollisionWithParticles(particle);
+    updateCollisionWithAvatars(particle);
 }
 
 void ParticleCollisionSystem::updateCollisionWithVoxels(Particle* particle) {
@@ -91,8 +96,76 @@ void ParticleCollisionSystem::updateCollisionWithParticles(Particle* particle) {
     }
 }
 
+void ParticleCollisionSystem::updateCollisionWithAvatars(Particle* particle) {
+
+    //printf("updateCollisionWithAvatars()...\n");
+    glm::vec3 center = particle->getPosition() * (float)TREE_SCALE;
+    float radius = particle->getRadius() * (float)TREE_SCALE;
+    const float VOXEL_ELASTICITY = 1.4f;
+    const float VOXEL_DAMPING = 0.0;
+    const float VOXEL_COLLISION_FREQUENCY = 0.5f;
+    glm::vec3 penetration;
+    const PalmData* collidingPalm = NULL;
+    
+    // first check the selfAvatar if set...
+    if (_selfAvatar) {
+        AvatarData* avatar = (AvatarData*)_selfAvatar;
+        //printf("updateCollisionWithAvatars()..._selfAvatar=%p\n", avatar);
+        
+        // check hands...
+        const HandData* handData = avatar->getHandData();
+
+        // if the particle penetrates the hand, then apply a hard collision
+        if (handData->findSpherePenetration(center, radius, penetration, collidingPalm)) {
+            penetration /= (float)TREE_SCALE;
+            updateCollisionSound(particle, penetration, VOXEL_COLLISION_FREQUENCY);
+    
+            // determine if the palm that collided was moving, if so, then we add that palm velocity as well...
+            glm::vec3 addedVelocity = NO_ADDED_VELOCITY;
+            if (collidingPalm) {
+                glm::vec3 palmVelocity = collidingPalm->getVelocity() / (float)TREE_SCALE;
+                //printf("collidingPalm Velocity=%f,%f,%f\n", palmVelocity.x, palmVelocity.y, palmVelocity.z);
+                addedVelocity = palmVelocity;
+            }
+
+            applyHardCollision(particle, penetration, VOXEL_ELASTICITY, VOXEL_DAMPING, addedVelocity);
+        }
+    }
+
+    // loop through all the other avatars for potential interactions...
+    NodeList* nodeList = NodeList::getInstance();
+    for (NodeList::iterator node = nodeList->begin(); node != nodeList->end(); node++) {
+        //qDebug() << "updateCollisionWithAvatars()... node:" << *node << "\n";
+        if (node->getLinkedData() && node->getType() == NODE_TYPE_AGENT) {
+            AvatarData* avatar = (AvatarData*)node->getLinkedData();
+            //printf("updateCollisionWithAvatars()...avatar=%p\n", avatar);
+            
+            // check hands...
+            const HandData* handData = avatar->getHandData();
+
+            // if the particle penetrates the hand, then apply a hard collision
+            if (handData->findSpherePenetration(center, radius, penetration, collidingPalm)) {
+                penetration /= (float)TREE_SCALE;
+                updateCollisionSound(particle, penetration, VOXEL_COLLISION_FREQUENCY);
+
+                // determine if the palm that collided was moving, if so, then we add that palm velocity as well...
+                glm::vec3 addedVelocity = NO_ADDED_VELOCITY;
+                if (collidingPalm) {
+                    glm::vec3 palmVelocity = collidingPalm->getVelocity() / (float)TREE_SCALE;
+                    //printf("collidingPalm Velocity=%f,%f,%f\n", palmVelocity.x, palmVelocity.y, palmVelocity.z);
+                    addedVelocity = palmVelocity;
+                }
+
+                applyHardCollision(particle, penetration, VOXEL_ELASTICITY, VOXEL_DAMPING, addedVelocity);
+
+            }
+        }
+    }
+}
+
+
 void ParticleCollisionSystem::applyHardCollision(Particle* particle, const glm::vec3& penetration, 
-                                float elasticity, float damping) {
+                                                 float elasticity, float damping, const glm::vec3& addedVelocity) {
     //
     //  Update the avatar in response to a hard collision.  Position will be reset exactly
     //  to outside the colliding surface.  Velocity will be modified according to elasticity.
@@ -112,6 +185,7 @@ void ParticleCollisionSystem::applyHardCollision(Particle* particle, const glm::
     if (penetrationLength > EPSILON) {
         glm::vec3 direction = penetration / penetrationLength;
         velocity -= glm::dot(velocity, direction) * direction * elasticity;
+        velocity += addedVelocity;
         velocity *= glm::clamp(1.f - damping, 0.0f, 1.0f);
         if (glm::length(velocity) < HALTING_VELOCITY) {
             // If moving really slowly after a collision, and not applying forces, stop altogether

libraries/particles/src/ParticleCollisionSystem.h

@@ -22,18 +22,22 @@
 #include "Particle.h"
 
 class AbstractAudioInterface;
+class AvatarData;
 class ParticleEditPacketSender;
 class ParticleTree;
 class VoxelTree;
 
+const glm::vec3 NO_ADDED_VELOCITY = glm::vec3(0);
+
 class ParticleCollisionSystem {
 public:
     ParticleCollisionSystem(ParticleEditPacketSender* packetSender = NULL, ParticleTree* particles = NULL, 
                                 VoxelTree* voxels = NULL, 
-                                AbstractAudioInterface* audio = NULL);
+                                AbstractAudioInterface* audio = NULL,
+                                AvatarData* selfAvatar = NULL);
 
     void init(ParticleEditPacketSender* packetSender, ParticleTree* particles, VoxelTree* voxels, 
-                                AbstractAudioInterface* audio = NULL);
+                                AbstractAudioInterface* audio = NULL, AvatarData* selfAvatar = NULL);
                                 
     ~ParticleCollisionSystem();
 
@@ -41,7 +45,9 @@ public:
     void checkParticle(Particle* particle);
     void updateCollisionWithVoxels(Particle* particle);
     void updateCollisionWithParticles(Particle* particle);
-    void applyHardCollision(Particle* particle, const glm::vec3& penetration, float elasticity, float damping);
+    void updateCollisionWithAvatars(Particle* particle);
+    void applyHardCollision(Particle* particle, const glm::vec3& penetration, float elasticity, float damping, 
+                            const glm::vec3& addedVelocity = NO_ADDED_VELOCITY);
     void updateCollisionSound(Particle* particle, const glm::vec3 &penetration, float frequency);
 
 private:
@@ -52,6 +58,7 @@ private:
     ParticleTree* _particles;
     VoxelTree* _voxels;
     AbstractAudioInterface* _audio;
+    AvatarData* _selfAvatar;
 };
 
 #endif /* defined(__hifi__ParticleCollisionSystem__) */