From 8df6a7e05666b026efd29a2215e338a39ce42359 Mon Sep 17 00:00:00 2001
From: Jeffrey Ventrella <jeffreyventrella@gmail.com>
Date: Tue, 23 Apr 2013 11:12:38 -0700
Subject: [PATCH] added collision code for avatar (not operational yet). and
 some cleanup.

---
 interface/src/Head.cpp | 121 ++++++++++++++++-------------------------
 interface/src/Head.h   |  29 ++++++++--
 interface/src/main.cpp |   3 +
 3 files changed, 73 insertions(+), 80 deletions(-)

diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp
index d1bbf5e65b..28cc3f8ddc 100644
--- a/interface/src/Head.cpp
+++ b/interface/src/Head.cpp
@@ -120,7 +120,13 @@ 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;
+
     if (iris_texture.size() == 0) {
         switchToResourcesParentIfRequired();
         unsigned error = lodepng::decode(iris_texture, iris_texture_width, iris_texture_height, iris_texture_file);
@@ -129,16 +135,12 @@ Head::Head(bool isMine) {
         }
     }
 		
-    //--------------------------------------------------
-    // test... just slam them into random positions...
-    //--------------------------------------------------
-    _otherAvatarHandPosition[ 0 ] = glm::vec3(  0.0f, 0.3f,  2.0f );
-    _otherAvatarHandPosition[ 1 ] = glm::vec3(  4.0f, 0.3f,  2.0f );
-    _otherAvatarHandPosition[ 2 ] = glm::vec3(  2.0f, 0.3f,  2.0f );
-    _otherAvatarHandPosition[ 3 ] = glm::vec3(  1.0f, 0.3f, -4.0f );
-    _otherAvatarHandPosition[ 4 ] = glm::vec3( -2.0f, 0.3f, -2.0f );
+    for (int o=0; o<MAX_OTHER_AVATARS; o++) {
+        _otherAvatarHandPosition[o] = glm::vec3( 0.0f, 0.0f, 0.0f );
+    }
 }
 
+
 Head::Head(const Head &otherAvatar) {
  
     _velocity               = otherAvatar._velocity;
@@ -304,7 +306,6 @@ void Head::setMousePressed( bool d ) {
 	_mousePressed = d;
 }
 
-
 void Head::simulate(float deltaTime) {
     
     //-------------------------------------------------------------
@@ -313,9 +314,6 @@ void Head::simulate(float deltaTime) {
     //-------------------------------------------------------------
     if ( _isMine )
     {
-        //-------------------------------------
-        // DEBUG - other avatars...
-        //-------------------------------------
         _closestOtherAvatar = -1;
         float closestDistance = 10000.0f;
         
@@ -350,33 +348,13 @@ void Head::simulate(float deltaTime) {
             }
         }
         
-        /*
-        ///for testing only (prior to having real avs working)
-        for (int o=0; o<NUM_OTHER_AVATARS; o++) {
-            //-------------------------------------
-            // test other avs for proximity...
-            //-------------------------------------
-            glm::vec3 v( _bone[ AVATAR_BONE_RIGHT_SHOULDER ].position );
-            v -= _DEBUG_otherAvatarListPosition[o];
-            
-            float distance = glm::length( v );
-
-            if ( distance < _maxArmLength ) {
-                if ( distance < closestDistance ) {
-                    closestDistance = distance;
-                    _closestOtherAvatar = o;
-                }
-            }
-        }
-        */
-        
     }//if ( _isMine )
         
     if ( usingBigSphereCollisionTest ) {
         //--------------------------------------------------------------
         // test for avatar collision response (using a big sphere :)
         //--------------------------------------------------------------
-        updateBigSphereCollisionTest(deltaTime);
+        updateAvatarCollisionDetectionAndResponse(_TEST_bigSpherePosition, _TEST_bigSphereRadius, deltaTime);
     }
     
     if ( AVATAR_GRAVITY ) {
@@ -576,26 +554,50 @@ void Head::simulate(float deltaTime) {
     }
 }
       
+      
+float Head::getGirth() {
+    return COLLISION_BODY_RADIUS;
+}
+
+float Head::getHeight() {
+    return COLLISION_HEIGHT;
+}
 	  
+      
+glm::vec3 Head::getBodyUpDirection() {
+    return _orientation.getUp();
+}
+
+      
+bool Head::testForCollision( glm::vec3 collisionPosition, float collisionGirth, float collisionHeight, glm::vec3 collisionUpVector ) {
+    _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;
+    
+    return false;
+}
+
 	  
 	  
 //--------------------------------------------------------------------------------
 // This is a workspace for testing avatar body collision detection and response
 //--------------------------------------------------------------------------------
-void Head::updateBigSphereCollisionTest( float deltaTime ) {
+void Head::updateAvatarCollisionDetectionAndResponse( glm::vec3 collisionPosition, float collisionRadius, float deltaTime ) {
 
     float myBodyApproximateBoundingRadius = 1.0f;
-    glm::vec3 vectorFromMyBodyToBigSphere(_bodyPosition - _TEST_bigSpherePosition);
+    glm::vec3 vectorFromMyBodyToBigSphere(_bodyPosition - collisionPosition);
     bool jointCollision = false;
 
     float distanceToBigSphere = glm::length(vectorFromMyBodyToBigSphere);
-    if ( distanceToBigSphere < myBodyApproximateBoundingRadius + _TEST_bigSphereRadius)
+    if ( distanceToBigSphere < myBodyApproximateBoundingRadius + collisionRadius )
     {
         for (int b=0; b<NUM_AVATAR_BONES; b++) 
         {
-            glm::vec3 vectorFromJointToBigSphereCenter(_bone[b].springyPosition - _TEST_bigSpherePosition);
+            glm::vec3 vectorFromJointToBigSphereCenter(_bone[b].springyPosition - collisionPosition);
             float distanceToBigSphereCenter = glm::length(vectorFromJointToBigSphereCenter);
-            float combinedRadius = _bone[b].radius + _TEST_bigSphereRadius;
+            float combinedRadius = _bone[b].radius + collisionRadius;
             if ( distanceToBigSphereCenter < combinedRadius ) 
             {
                 jointCollision = true;
@@ -608,7 +610,7 @@ void Head::updateBigSphereCollisionTest( float deltaTime ) {
                                           
                     _bone[b].springyVelocity += collisionForce *  30.0f * deltaTime;
                     _velocity                += collisionForce * 100.0f * deltaTime;
-                    _bone[b].springyPosition = _TEST_bigSpherePosition + directionVector * combinedRadius;
+                    _bone[b].springyPosition = collisionPosition + directionVector * combinedRadius;
                 }
             }
         }
@@ -665,19 +667,6 @@ void Head::render(int faceToFace) {
 	//---------------------------------------------------------------------------
     if ( _isMine )
     {
-        /*
-        //---------------------------------------------------
-        // render other avatars (DEBUG TEST)
-        //---------------------------------------------------
-        for (int o=0; o<_numOtherAvatarsInView; o++) {
-            glPushMatrix();
-                glTranslatef( _otherAvatarHandPosition[o].x, _otherAvatarHandPosition[o].y, _otherAvatarHandPosition[o].z );
-                glScalef( 0.03, 0.03, 0.03 );
-                glutSolidSphere( 1, 10, 10 );
-            glPopMatrix();
-        }
-        */
-
         if (_usingBodySprings) {
             if ( _closestOtherAvatar != -1 ) {					
 
@@ -1133,16 +1122,16 @@ void Head::updateHandMovement() {
     
 	_bone[ AVATAR_BONE_RIGHT_HAND ].position += transformedHandMovement;
     
-	//if holding hands, add a pull to the hand...
+    //-----------------------------------------------------
+	// if holding hands, apply the appropriate forces...
+    //-----------------------------------------------------
 	if ( _usingBodySprings ) {
 		if ( _closestOtherAvatar != -1 ) {	
 			if ( _mousePressed ) {
 
-    
 				glm::vec3 handToHandVector( _otherAvatarHandPosition[ _closestOtherAvatar ]);
 				handToHandVector -= _bone[ AVATAR_BONE_RIGHT_HAND ].position;
 				
-				//_bone[ AVATAR_BONE_RIGHT_HAND ].springyVelocity -= handPull;				
 				_bone[ AVATAR_BONE_RIGHT_HAND ].position = _otherAvatarHandPosition[ _closestOtherAvatar ];				
 			}
 		}
@@ -1154,7 +1143,6 @@ void Head::updateHandMovement() {
 	glm::vec3 armVector = _bone[ AVATAR_BONE_RIGHT_HAND ].position;
 	armVector -= _bone[ AVATAR_BONE_RIGHT_SHOULDER ].position;
 
-
 	//-------------------------------------------------------------------------------
 	// test to see if right hand is being dragged beyond maximum arm length
 	//-------------------------------------------------------------------------------
@@ -1175,30 +1163,13 @@ void Head::updateHandMovement() {
 		constrainedPosition += armVector;
 		_bone[ AVATAR_BONE_RIGHT_HAND ].position = constrainedPosition;
 	}
-	
-	/*
-	//-------------------------------------------------------------------------------
-	// keep arm from going through av body...
-	//-------------------------------------------------------------------------------	
-	glm::vec3 adjustedArmVector = _bone[ AVATAR_BONE_RIGHT_HAND ].position;
-	adjustedArmVector -= _bone[ AVATAR_BONE_RIGHT_SHOULDER ].position;
-	
-	float rightComponent = glm::dot( adjustedArmVector, avatar.orientation.getRight() );
-
-	if ( rightComponent < 0.0 )
-	{
-		_bone[ AVATAR_BONE_RIGHT_HAND ].position -= avatar.orientation.getRight() * rightComponent;
-	}	
-	*/
-	
+		
 	//-----------------------------------------------------------------------------
 	// set elbow position 
 	//-----------------------------------------------------------------------------
 	glm::vec3 newElbowPosition = _bone[ AVATAR_BONE_RIGHT_SHOULDER ].position;
 	newElbowPosition += armVector * ONE_HALF;
-//glm::vec3 perpendicular = glm::cross( frontDirection, armVector );
 	glm::vec3 perpendicular = glm::cross( _orientation.getFront(), armVector );
-
 	newElbowPosition += perpendicular * ( 1.0f - ( _maxArmLength / distance ) ) * ONE_HALF;
 	_bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].position = newElbowPosition;
 
diff --git a/interface/src/Head.h b/interface/src/Head.h
index 37488699b7..7d6243310b 100644
--- a/interface/src/Head.h
+++ b/interface/src/Head.h
@@ -29,6 +29,9 @@ 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_HEIGHT = 1.5;
+
 enum eyeContactTargets {LEFT_EYE, RIGHT_EYE, MOUTH};
 
 #define FWD 0
@@ -41,7 +44,7 @@ enum eyeContactTargets {LEFT_EYE, RIGHT_EYE, MOUTH};
 #define ROT_RIGHT 7 
 #define MAX_DRIVE_KEYS 8
 
-#define MAX_OTHER_AVATARS 50 // temporary - for testing purposes!
+#define MAX_OTHER_AVATARS 10 // temporary - for testing purposes!
 
 enum AvatarMode
 {
@@ -81,6 +84,15 @@ enum AvatarBoneID
 	NUM_AVATAR_BONES
 };
 
+struct AvatarCollisionElipsoid
+{
+    bool      colliding;
+    glm::vec3 position;
+    float     girth;
+    float     height;
+    glm::vec3 upVector;
+};
+
 struct AvatarBone
 {
 	AvatarBoneID parent;				// which bone is this bone connected to?
@@ -175,8 +187,11 @@ class Head : public AvatarData {
 		glm::vec3 getHeadLookatDirectionUp();
 		glm::vec3 getHeadLookatDirectionRight();
 		glm::vec3 getHeadPosition();
-		glm::vec3 getBonePosition( AvatarBoneID b );		
-		
+		glm::vec3 getBonePosition( AvatarBoneID b );	
+        glm::vec3 getBodyUpDirection();
+        float getGirth();
+        float getHeight();
+        
 		AvatarMode getMode();
 		
 		void setMousePressed( bool pressed ); 
@@ -194,6 +209,8 @@ class Head : public AvatarData {
         void setAverageLoudness(float al) {_head.averageLoudness = al;};
         void setLoudness(float l) {_head.loudness = l;};
         
+        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
@@ -236,7 +253,9 @@ class Head : public AvatarData {
         int         _driveKeys[MAX_DRIVE_KEYS];
         GLUquadric* _sphere;
         float       _renderYaw;
-        float       _renderPitch; //   Pitch from view frustum when this is own head.
+        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
@@ -253,7 +272,7 @@ class Head : public AvatarData {
 		void initializeBodySprings();
 		void updateBodySprings( float deltaTime );
 		void calculateBoneLengths();
-        void updateBigSphereCollisionTest( float deltaTime );
+        void updateAvatarCollisionDetectionAndResponse( glm::vec3, float radius, float deltaTime );
         void readSensors();
         void renderBoneAsBlock( AvatarBoneID b );
 
diff --git a/interface/src/main.cpp b/interface/src/main.cpp
index b3883f6e97..c13b20ecc0 100644
--- a/interface/src/main.cpp
+++ b/interface/src/main.cpp
@@ -1510,6 +1510,9 @@ void idle(void) {
 			{
                 Head *avatar = (Head *)agent->getLinkedData();
                 avatar->simulate(deltaTime);
+                
+                //not ready yet...
+                //myAvatar.testForCollision( avatar->getBodyPosition(), avatar->getGirth(), avatar->getHeight(), avatar->getBodyUpDirection() );
             }
         }