From 61d7a82fc6020b79f5916823788a4467e38632fc Mon Sep 17 00:00:00 2001
From: Jeffrey Ventrella <jeffreyventrella@gmail.com>
Date: Tue, 23 Apr 2013 20:44:12 -0700
Subject: [PATCH] made lots of changes in the logic of how my avatar encounters
 other avatars and detects hand state

---
 interface/src/Head.cpp | 107 +++++++++++++++++++----------------------
 interface/src/Head.h   |  97 ++++++++++++++++++++-----------------
 2 files changed, 102 insertions(+), 102 deletions(-)

diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp
index f4269a47bd..f93389511c 100644
--- a/interface/src/Head.cpp
+++ b/interface/src/Head.cpp
@@ -50,7 +50,7 @@ Head::Head(bool isMine) {
 	_velocity           = glm::vec3( 0.0, 0.0, 0.0 );
 	_thrust		        = glm::vec3( 0.0, 0.0, 0.0 );
     _rotation           = glm::quat( 0.0f, 0.0f, 0.0f, 0.0f );	
-	_closestOtherAvatar = 0;
+	_nearOtherAvatar    = false;
 	_bodyYaw            = -90.0;
 	_bodyPitch          = 0.0;
 	_bodyRoll           = 0.0;
@@ -62,7 +62,7 @@ Head::Head(bool isMine) {
     //_transmitterTimer   = 0;
     _transmitterHz      = 0.0;
     _transmitterPackets = 0;
-    _numOtherAvatarsInView = 0;
+    //_numOtherAvatars    = 0;
 
     initializeSkeleton();
     
@@ -119,13 +119,11 @@ Head::Head(bool isMine) {
     _renderPitch             = 0.0;
 	
 	_sphere = NULL;
-
-    _collisionElipsoid.colliding = false;
-    _collisionElipsoid.position  = glm::vec3( 0.0, 0.0, 0.0 );
-    _collisionElipsoid.upVector  = glm::vec3( 0.0, 0.0, 0.0 );
-    _collisionElipsoid.girth     = 0.0;
-    _collisionElipsoid.height    = 0.0;
-
+    
+    _handHolding.position = glm::vec3( 0.0, 0.0, 0.0 );
+    _handHolding.velocity = glm::vec3( 0.0, 0.0, 0.0 );
+    _handHolding.force    = 10.0f;
+    
     if (iris_texture.size() == 0) {
         switchToResourcesParentIfRequired();
         unsigned error = lodepng::decode(iris_texture, iris_texture_width, iris_texture_height, iris_texture_file);
@@ -133,20 +131,19 @@ Head::Head(bool isMine) {
             printLog("error %u: %s\n", error, lodepng_error_text(error));
         }
     }
-		
-    for (int o=0; o<MAX_OTHER_AVATARS; o++) {
-        _otherAvatarHandPosition[o] = glm::vec3( 0.0f, 0.0f, 0.0f );
-        _otherAvatarHandState   [o] = 0;
-    }
+
+    _otherAvatar.handPosition = glm::vec3( 0.0f, 0.0f, 0.0f );
+    _otherAvatar.handState = 0;
 }
 
 
+
 Head::Head(const Head &otherAvatar) {
  
     _velocity               = otherAvatar._velocity;
 	_thrust                 = otherAvatar._thrust;
     _rotation               = otherAvatar._rotation;
-	_closestOtherAvatar     = otherAvatar._closestOtherAvatar;
+	_nearOtherAvatar        = otherAvatar._nearOtherAvatar;
 	_bodyYaw                = otherAvatar._bodyYaw;
 	_bodyPitch              = otherAvatar._bodyPitch;
 	_bodyRoll               = otherAvatar._bodyRoll;
@@ -308,12 +305,12 @@ void Head::simulate(float deltaTime) {
     //-------------------------------------------------------------
     if ( _isMine )
     {
-        _closestOtherAvatar = -1;
+        _nearOtherAvatar = false;
         float closestDistance = 10000.0f;
         
         AgentList * agentList = AgentList::getInstance();
         
-        _numOtherAvatarsInView =0;
+        //_numOtherAvatars = 0;
 
         for(std::vector<Agent>::iterator agent = agentList->getAgents().begin();
             agent != agentList->getAgents().end();
@@ -321,7 +318,8 @@ void Head::simulate(float deltaTime) {
             if (( agent->getLinkedData() != NULL && ( agent->getType() == AGENT_TYPE_AVATAR ) )) {
                 Head *otherAvatar = (Head *)agent->getLinkedData();
                 
-                if ( _numOtherAvatarsInView < MAX_OTHER_AVATARS ) {
+               // if ( _numOtherAvatars < MAX_OTHER_AVATARS ) 
+               {
                  
                     //------------------------------------------------------
                     // check for collisions with other avatars and respond 
@@ -334,27 +332,20 @@ void Head::simulate(float deltaTime) {
                         otherAvatar->getBodyUpDirection(),
                         deltaTime
                     );
-                 
-                    //-----------------------------------------------------------
-                    // test other avatar hand position for proximity and state
-                    //-----------------------------------------------------------
-                    _otherAvatarHandPosition[ _numOtherAvatarsInView ] = otherAvatar->getBonePosition( AVATAR_BONE_RIGHT_HAND );
-                    _otherAvatarHandState   [ _numOtherAvatarsInView ] = (int)otherAvatar->getHandState();
-
-                    glm::vec3 v( _bone[ AVATAR_BONE_RIGHT_SHOULDER ].position );
-                    v -= _otherAvatarHandPosition[ _numOtherAvatarsInView ];
                     
-                    //if ( otherAvatar->getHandState() == 1 ) 
-                    {
-                        printf( "otherAvatar->getHandState() is %d\n", (int)otherAvatar->getHandState() );
-                    }
+                    //-------------------------------------------------
+                    // test other avatar hand position for proximity 
+                    //------------------------------------------------
+                    glm::vec3 v( _bone[ AVATAR_BONE_RIGHT_SHOULDER ].position );
+                    v -= otherAvatar->getBonePosition( AVATAR_BONE_RIGHT_HAND );
                     
                     float distance = glm::length( v );
                     if ( distance < _maxArmLength ) {
                         if ( distance < closestDistance ) {
                             closestDistance = distance;
-                            _closestOtherAvatar = _numOtherAvatarsInView;
-                            _numOtherAvatarsInView++;
+                            _nearOtherAvatar = true;
+                            _otherAvatar.handPosition = otherAvatar->getBonePosition( AVATAR_BONE_RIGHT_HAND );
+                            _otherAvatar.handState = (int)otherAvatar->getHandState();
                         }
                     }
                 }
@@ -398,7 +389,7 @@ void Head::simulate(float deltaTime) {
 	// reset hand and arm positions according to hand movement
 	//------------------------------------------------------------
 	if (_usingBodySprings) {
-		updateHandMovement();
+		updateHandMovement( deltaTime );
 		updateBodySprings( deltaTime );
 	}
 	
@@ -652,7 +643,6 @@ void Head::render(bool lookingInMirror) {
         glutSolidSphere( 1, 10, 10 );
 	glPopMatrix();
     
-    
     if ( usingBigSphereCollisionTest ) {
         //---------------------------------------------------
         // show TEST big sphere 
@@ -681,10 +671,10 @@ void Head::render(bool lookingInMirror) {
     if ( _isMine )
     {
         if (_usingBodySprings) {
-            if ( _closestOtherAvatar != -1 ) {					
+            if ( _nearOtherAvatar ) {					
 
                 glm::vec3 v1( _bone[ AVATAR_BONE_RIGHT_HAND ].position );
-                glm::vec3 v2( _otherAvatarHandPosition[ _closestOtherAvatar ] );
+                glm::vec3 v2( _otherAvatar.handPosition );
                 
                 glLineWidth( 8.0 );
                 glColor4f( 0.7f, 0.4f, 0.1f, 0.6 );
@@ -987,12 +977,6 @@ void Head::initializeSkeleton() {
 	updateSkeleton();
 }
 
-/*
-int Head::getHandState() {
-    return _handState;
-}
-*/
-
 void Head::calculateBoneLengths() {
 	for (int b=0; b<NUM_AVATAR_BONES; b++) {
 		_bone[b].length = glm::length( _bone[b].defaultPosePosition );
@@ -1134,7 +1118,7 @@ glm::vec3 Head::getBonePosition( AvatarBoneID b ) {
 
 
 
-void Head::updateHandMovement() {
+void Head::updateHandMovement( float deltaTime ) {
 	glm::vec3 transformedHandMovement;
 	    
 	transformedHandMovement 
@@ -1146,22 +1130,31 @@ void Head::updateHandMovement() {
     
     setHandState(_mousePressed);
     
-    
-    //printf( "_handState = %d", _handState );    
-    
-	//if holding hands, add a pull to the hand...
-	if ( _usingBodySprings ) {
-		if ( _closestOtherAvatar != -1 ) {	
-			if ( _mousePressed ) {
-            
-				glm::vec3 handToHandVector( _otherAvatarHandPosition[ _closestOtherAvatar ]);
-				handToHandVector -= _bone[ AVATAR_BONE_RIGHT_HAND ].position;
+    //---------------------------------------------------------------------
+	// if holding hands with another avatar, add a force to the hand...
+    //---------------------------------------------------------------------
+    if (( getHandState() == 1 )
+    ||  ( _otherAvatar.handState == 1 )) {
+        //if ( _usingBodySprings ) 
+        {
+            if ( _nearOtherAvatar ) {	            
+                 
+                glm::vec3 vectorToOtherHand = _otherAvatar.handPosition - _handHolding.position;
+                glm::vec3 vectorToMyHand    = _bone[ AVATAR_BONE_RIGHT_HAND ].position        - _handHolding.position;
 				
-				_bone[ AVATAR_BONE_RIGHT_HAND ].position = _otherAvatarHandPosition[ _closestOtherAvatar ];				
+                _handHolding.velocity *= 0.7;
+				_handHolding.velocity += ( vectorToOtherHand + vectorToMyHand ) * _handHolding.force * deltaTime;	
+                _handHolding.position += _handHolding.velocity;
+                
+                _bone[ AVATAR_BONE_RIGHT_HAND ].position = _handHolding.position;		
 			} 
 		}
-	}
-    
+    }
+    else {
+        _handHolding.position = _bone[ AVATAR_BONE_RIGHT_HAND ].position;
+        _handHolding.velocity = glm::vec3( 0.0, 0.0, 0.0 );
+    }
+        
 	//-------------------------------------------------------------------------------
 	// determine the arm vector
 	//-------------------------------------------------------------------------------
diff --git a/interface/src/Head.h b/interface/src/Head.h
index 3281c57088..428ba2228a 100644
--- a/interface/src/Head.h
+++ b/interface/src/Head.h
@@ -29,7 +29,7 @@ const float YAW_MAG         = 300.0;
 const float TEST_YAW_DECAY  = 5.0;
 const float LIN_VEL_DECAY   = 5.0;
 
-const float COLLISION_BODY_RADIUS = 0.3;
+const float COLLISION_BODY_RADIUS = 0.1;
 const float COLLISION_HEIGHT = 1.5;
 
 enum eyeContactTargets {LEFT_EYE, RIGHT_EYE, MOUTH};
@@ -93,6 +93,20 @@ struct AvatarCollisionElipsoid
     glm::vec3 upVector;
 };
 
+struct AvatarHandHolding
+{
+    glm::vec3 position;
+    glm::vec3 velocity;
+    float     force;
+};
+
+struct OtherAvatar
+{
+    glm::vec3 handPosition;
+    int       handState;
+};
+
+
 struct AvatarBone
 {
 	AvatarBoneID parent;				// which bone is this bone connected to?
@@ -191,13 +205,11 @@ class Head : public AvatarData {
 		void startHandMovement();
 		void stopHandMovement();
 		void setHandMovementValues( glm::vec3 movement );
-		void updateHandMovement();
+		void updateHandMovement( float deltaTime );
         
         float getAverageLoudness() {return _head.averageLoudness;};
         void setAverageLoudness(float al) {_head.averageLoudness = al;};
-        
-        //bool testForCollision( glm::vec3 collisionPosition, float collisionGirth, float collisionHeight, glm::vec3 collisionUpVector );
-        
+         
         void SetNewHeadTarget(float, float);
     
         //  Set what driving keys are being pressed to control thrust levels
@@ -209,48 +221,38 @@ class Head : public AvatarData {
         void addThrust(glm::vec3 newThrust) { _thrust += newThrust; };
         glm::vec3 getThrust() { return _thrust; };
     
-        //
         //  Related to getting transmitter UDP data used to animate the avatar hand
-        //
-
         void processTransmitterData(unsigned char * packetData, int numBytes);
         float getTransmitterHz() { return _transmitterHz; };
     
     private:
-        AvatarHead  _head;    
-        bool        _isMine;
-        glm::vec3   _TEST_bigSpherePosition;
-        float       _TEST_bigSphereRadius;
-		glm::vec3	_otherAvatarHandPosition[ MAX_OTHER_AVATARS ];
-		int         _otherAvatarHandState   [ MAX_OTHER_AVATARS ];
-		bool        _mousePressed;
-		float       _bodyYawDelta;
-		int         _closestOtherAvatar;
-		bool        _usingBodySprings;
-		glm::vec3   _movedHandOffset;
-		float       _springVelocityDecay;
-		float       _springForce;
-        glm::quat   _rotation; // the rotation of the avatar body as a whole expressed as a quaternion
-		AvatarBone	_bone[ NUM_AVATAR_BONES ];
-		AvatarMode  _mode;
-        glm::dvec3	_velocity;
-        glm::vec3	_thrust;
-        float		_maxArmLength;
-        Orientation	_orientation;
-        int         _numOtherAvatarsInView;
-        int         _driveKeys[MAX_DRIVE_KEYS];
-        GLUquadric* _sphere;
-        float       _renderYaw;
-        float       _renderPitch; //   Pitch from view frustum when this is own head
-        
-        AvatarCollisionElipsoid _collisionElipsoid;
-    
-        //
-        //  Related to getting transmitter UDP data used to animate the avatar hand
-        //
-        timeval _transmitterTimer;
-        float   _transmitterHz;
-        int     _transmitterPackets;
+        AvatarHead        _head;    
+        bool              _isMine;
+        glm::vec3         _TEST_bigSpherePosition;
+        float             _TEST_bigSphereRadius;
+        OtherAvatar       _otherAvatar;
+		bool              _mousePressed;
+		float             _bodyYawDelta;
+		bool              _nearOtherAvatar;
+		bool              _usingBodySprings;
+		glm::vec3         _movedHandOffset;
+		float             _springVelocityDecay;
+		float             _springForce;
+        glm::quat         _rotation; // the rotation of the avatar body as a whole expressed as a quaternion
+		AvatarBone	      _bone[ NUM_AVATAR_BONES ];
+		AvatarMode        _mode;
+        AvatarHandHolding _handHolding;
+        glm::dvec3        _velocity;
+        glm::vec3	      _thrust;
+        float		      _maxArmLength;
+        Orientation	      _orientation;
+        int               _driveKeys[MAX_DRIVE_KEYS];
+        GLUquadric*       _sphere;
+        float             _renderYaw;
+        float             _renderPitch; //   Pitch from view frustum when this is own head
+        timeval           _transmitterTimer;
+        float             _transmitterHz;
+        int               _transmitterPackets;
         
         //-----------------------------
         // private methods...
@@ -260,11 +262,16 @@ class Head : public AvatarData {
 		void initializeBodySprings();
 		void updateBodySprings( float deltaTime );
 		void calculateBoneLengths();
-        void updateAvatarCollisionDetectionAndResponse( glm::vec3 collisionPosition, float collisionGirth, float collisionHeight, glm::vec3 collisionUpVector, float deltaTime );
-        
         void readSensors();
         void renderBoneAsBlock( AvatarBoneID b );
-
+        void updateAvatarCollisionDetectionAndResponse
+        ( 
+            glm::vec3 collisionPosition, 
+            float     collisionGirth, 
+            float     collisionHeight, 
+            glm::vec3 collisionUpVector, 
+            float     deltaTime 
+        );
 };
 
 #endif