Merge branch 'master' of github.com:worklist/hifi

domain/src/main.cpp

@@ -25,6 +25,8 @@
 #include <fcntl.h>
 #include <map>
 #include "AgentList.h"
+#include "AgentTypes.h"
+#include <PacketHeaders.h>
 #include "SharedUtil.h"
 
 #ifdef _WIN32
@@ -46,7 +48,7 @@ const int LOGOFF_CHECK_INTERVAL = 5000;
 #define DEBUG_TO_SELF 0
 
 int lastActiveCount = 0;
-AgentList agentList('D', DOMAIN_LISTEN_PORT);
+AgentList agentList(AGENT_TYPE_DOMAIN, DOMAIN_LISTEN_PORT);
 
 unsigned char * addAgentToBroadcastPacket(unsigned char *currentPosition, Agent *agentToAdd) {
     *currentPosition++ = agentToAdd->getType();
@@ -82,7 +84,7 @@ int main(int argc, const char * argv[])
     char agentType;
     
     unsigned char *broadcastPacket = new unsigned char[MAX_PACKET_SIZE];
-    *broadcastPacket = 'D';
+    *broadcastPacket = PACKET_HEADER_DOMAIN;
     
     unsigned char *currentBufferPos;
     unsigned char *startPointer;

injector/src/injector.cpp

@@ -20,6 +20,8 @@
 #include "UDPSocket.h"
 #include "UDPSocket.cpp"
 #include <SharedUtil.h>
+#include <PacketHeaders.h>
+
 
 char EC2_WEST_AUDIO_SERVER[] = "54.241.92.53";
 const int AUDIO_UDP_LISTEN_PORT = 55443;
@@ -121,7 +123,7 @@ void stream(void)
     int leadingBytes = 1 + (sizeof(float) * 4);
     unsigned char dataPacket[BUFFER_LENGTH_BYTES + leadingBytes];
     
-    dataPacket[0] = 'I';
+    dataPacket[0] = PACKET_HEADER_INJECT_AUDIO;
     unsigned char *currentPacketPtr = dataPacket + 1;
     
     for (int p = 0; p < 4; p++) {

interface/src/Audio.cpp

@@ -16,6 +16,7 @@
 #include <StdDev.h>
 #include <UDPSocket.h>
 #include <SharedUtil.h>
+#include <PacketHeaders.h>
 #include "Audio.h"
 #include "Util.h"
 
@@ -150,7 +151,7 @@ int audioCallback (const void *inputBuffer,
             // + 12 for 3 floats for position + float for bearing + 1 attenuation byte
             unsigned char dataPacket[BUFFER_LENGTH_BYTES + leadingBytes];
             
-            dataPacket[0] = 'I';
+            dataPacket[0] = PACKET_HEADER_INJECT_AUDIO;
             unsigned char *currentPacketPtr = dataPacket + 1;
             
             // memcpy the three float positions
@@ -230,19 +231,19 @@ int audioCallback (const void *inputBuffer,
     if (ringBuffer->getEndOfLastWrite() != NULL) {
         
         if (!ringBuffer->isStarted() && ringBuffer->diffLastWriteNextOutput() < PACKET_LENGTH_SAMPLES + JITTER_BUFFER_SAMPLES) {
-            printf("Held back, buffer has %d of %d samples required.\n", ringBuffer->diffLastWriteNextOutput(), PACKET_LENGTH_SAMPLES + JITTER_BUFFER_SAMPLES);
+            //printf("Held back, buffer has %d of %d samples required.\n", ringBuffer->diffLastWriteNextOutput(), PACKET_LENGTH_SAMPLES + JITTER_BUFFER_SAMPLES);
         } else if (ringBuffer->diffLastWriteNextOutput() < PACKET_LENGTH_SAMPLES) {
             ringBuffer->setStarted(false);
             
             starve_counter++;
             packetsReceivedThisPlayback = 0;
 
-            printf("Starved #%d\n", starve_counter);
+            //printf("Starved #%d\n", starve_counter);
             data->wasStarved = 10;      //   Frames to render the indication that the system was starved.
         } else {
             if (!ringBuffer->isStarted()) {
                 ringBuffer->setStarted(true);
-                printf("starting playback %3.1f msecs delayed, \n", (usecTimestampNow() - usecTimestamp(&firstPlaybackTimer))/1000.0);
+                printf("starting playback %3.1f msecs delayed \n", (usecTimestampNow() - usecTimestamp(&firstPlaybackTimer))/1000.0);
             } else {
                 //printf("pushing buffer\n");
             }
@@ -395,7 +396,7 @@ void *receiveAudioViaUDP(void *args) {
             if (totalPacketsReceived > 3) stdev.addValue(tDiff);
             if (stdev.getSamples() > 500) {
                 sharedAudioData->measuredJitter = stdev.getStDev();
-                printf("Avg: %4.2f, Stdev: %4.2f\n", stdev.getAverage(), sharedAudioData->measuredJitter);
+                //printf("Avg: %4.2f, Stdev: %4.2f\n", stdev.getAverage(), sharedAudioData->measuredJitter);
                 stdev.reset();
             }
             
@@ -403,7 +404,7 @@ void *receiveAudioViaUDP(void *args) {
             
             
             if (!ringBuffer->isStarted()) {
-                printf("Audio packet %d received at %6.0f\n", ++packetsReceivedThisPlayback, usecTimestampNow()/1000);
+                packetsReceivedThisPlayback++;
              }
             else {
                 //printf("Audio packet received at %6.0f\n", usecTimestampNow()/1000);

interface/src/Camera.cpp

@@ -0,0 +1,48 @@
+//-----------------------------------------------------------
+//
+// Created by Jeffrey Ventrella and added as a utility 
+// class for High Fidelity Code base, April 2013
+//
+//-----------------------------------------------------------
+
+#include "Camera.h"
+#include "Util.h"
+
+//------------------------
+Camera::Camera()
+{
+	mode			= CAMERA_MODE_THIRD_PERSON;
+	fieldOfView		= 60.0; // default
+	yaw				= 0.0;
+	pitch			= 0.0;
+	roll			= 0.0;
+	up				= 0.0;
+	distance		= 0.0;
+	targetPosition	= glm::dvec3( 0.0, 0.0, 0.0 );
+	position		= glm::dvec3( 0.0, 0.0, 0.0 );
+	orientation.setToIdentity();
+}
+
+
+
+//------------------------
+void Camera::update()
+{
+	double radian = ( yaw / 180.0 ) * PIE;
+
+	double x = distance *  sin( radian );
+	double z = distance * -cos( radian );
+	double y = -up;
+	
+	position = glm::dvec3( targetPosition );	
+	position += glm::dvec3( x, y, z );
+	
+	//------------------------------------------------------------------------
+	//geterate the ortho-normals for the orientation based on the Euler angles
+	//------------------------------------------------------------------------
+	orientation.setToIdentity();
+	orientation.yaw		( yaw	);
+	orientation.pitch	( pitch	);
+	orientation.roll	( roll	);
+}
+

interface/src/Camera.h

@@ -0,0 +1,63 @@
+//-----------------------------------------------------------
+//
+// Created by Jeffrey Ventrella and added as a utility 
+// class for High Fidelity Code base, April 2013
+//
+//-----------------------------------------------------------
+
+#ifndef __interface__camera__
+#define __interface__camera__
+
+#include "Vector3D.h"
+#include "Orientation.h"
+#include <glm/glm.hpp>
+
+enum CameraMode
+{
+	CAMERA_MODE_NULL = -1,
+	CAMERA_MODE_FIRST_PERSON,
+	CAMERA_MODE_THIRD_PERSON,
+	CAMERA_MODE_MY_OWN_FACE,
+	NUM_CAMERA_MODES
+};
+
+
+class Camera
+{
+public:
+	Camera();
+	
+	void update();
+	
+	void setMode			( CameraMode	m ) { mode				= m; }
+	void setYaw				( float			y ) { yaw				= y; }
+	void setPitch			( float			p ) { pitch				= p; }
+	void setRoll			( float			r ) { roll				= r; }
+	void setUp				( float			u ) { up				= u; }
+	void setDistance		( float			d ) { distance			= d; }
+	void setTargetPosition	( glm::vec3		t ) { targetPosition	= t; };
+	void setPosition		( glm::vec3		p ) { position			= p; };
+	void setOrientation		( Orientation	o ) { orientation.set(o); }
+
+	float		getYaw			() { return yaw;			}
+	float		getPitch		() { return pitch;			}
+	float		getRoll			() { return roll;			}
+	glm::vec3	getPosition		() { return position;		}
+	Orientation	getOrientation	() { return orientation;	}
+	CameraMode	getMode			() { return mode;			}
+
+private:
+
+	CameraMode	mode;
+	glm::vec3	position;
+	glm::vec3	targetPosition;
+	float		fieldOfView;
+	float		yaw;
+	float		pitch;
+	float		roll;
+	float		up;
+	float		distance;
+	Orientation	orientation;
+};
+
+#endif

interface/src/Hand.h

@@ -40,6 +40,7 @@ private:
     float transmitterHz;
     int transmitterPackets;
     bool renderPointer;
+	
 };
 
 

interface/src/Head.cpp

@@ -3,6 +3,7 @@
 //  interface
 //
 //  Created by Philip Rosedale on 9/11/12.
+//	adapted by Jeffrey Ventrella, starting on April 2, 2013
 //  Copyright (c) 2012 Physical, Inc.. All rights reserved.
 //
 
@@ -18,6 +19,7 @@
 using namespace std;
 
 float skinColor[] = {1.0, 0.84, 0.66};
+float lightBlue[] = { 0.7, 0.8, 1.0 };
 float browColor[] = {210.0/255.0, 105.0/255.0, 30.0/255.0};
 float mouthColor[] = {1, 0, 0};
 
@@ -38,11 +40,16 @@ vector<unsigned char> iris_texture;
 unsigned int iris_texture_width = 512;
 unsigned int iris_texture_height = 256;
 
+
+
+//---------------------------------------------------
 Head::Head()
 {
-    position = glm::vec3(0,0,0);
+	initializeAvatar();
-    velocity = glm::vec3(0,0,0);
+
-    thrust = glm::vec3(0,0,0);
+    position	= glm::vec3(0,0,0);
+    velocity	= glm::vec3(0,0,0);
+    thrust		= glm::vec3(0,0,0);
     
     for (int i = 0; i < MAX_DRIVE_KEYS; i++) driveKeys[i] = false; 
     
@@ -77,6 +84,8 @@ Head::Head()
     lastLoudness = 0.0;
     browAudioLift = 0.0;
     noise = 0;
+	
+	handOffset = glm::vec3( 0.0, 0.0, 0.0 );
     
     sphere = NULL;
     
@@ -91,7 +100,14 @@ Head::Head()
     }
 }
 
-Head::Head(const Head &otherHead) {
+
+
+
+//---------------------------------------------------
+Head::Head(const Head &otherHead) 
+{
+	initializeAvatar();
+
     position = otherHead.position;
     velocity = otherHead.velocity;
     thrust = otherHead.thrust;
@@ -141,26 +157,45 @@ Head::Head(const Head &otherHead) {
     hand = &newHand;
 }
 
-Head::~Head() {
+
-    if (sphere != NULL) {
+
+
+//---------------------------------------------------
+Head::~Head() 
+{
+    if (sphere != NULL) 
+	{
         gluDeleteQuadric(sphere);
     }
 }
 
-Head* Head::clone() const {
+
+
+//---------------------------------------------------
+Head* Head::clone() const 
+{
     return new Head(*this);
 }
 
+
+
+
+
+//---------------------------------------------------
 void Head::reset()
 {
     Pitch = Yaw = Roll = 0;
     leanForward = leanSideways = 0;
 }
 
+
+
+
+//this pertains to moving the head with the glasses 
+//---------------------------------------------------
 void Head::UpdatePos(float frametime, SerialInterface * serialInterface, int head_mirror, glm::vec3 * gravity)
 //  Using serial data, update avatar/render position and angles
 {
-    
     const float PITCH_ACCEL_COUPLING = 0.5;
     const float ROLL_ACCEL_COUPLING = -1.0;
     float measured_pitch_rate = serialInterface->getRelativeValue(PITCH_RATE);
@@ -201,64 +236,145 @@ void Head::UpdatePos(float frametime, SerialInterface * serialInterface, int hea
     } 
 }
 
+
+
+
+//---------------------------------------------------
+void Head::setAvatarPosition( float x, float y, float z )
+{
+	avatar.position = glm::vec3( x, y, z );
+}
+
+
+//---------------------------------------------------
 void Head::addLean(float x, float z) {
     //  Add Body lean as impulse 
     leanSideways += x;
     leanForward += z;
 }
 
+
+
+//---------------------------------------------------
 void Head::setLeanForward(float dist){
     leanForward = dist;
 }
 
+
+
+
+//---------------------------------------------------
 void Head::setLeanSideways(float dist){
     leanSideways = dist;
 }
 
-//  Simulate the head over time 
+
+
+
+
+
+//  Simulate the avatar over time 
+//---------------------------------------------------
 void Head::simulate(float deltaTime)
 {
-    
+	updateAvatarSkeleton();
+
+	/*
+    glm::vec3 forward
+	(
+		-sin( avatar.yaw	* PI_OVER_180 ),
+		 sin( avatar.pitch	* PI_OVER_180 ),
+		 cos( avatar.roll	* PI_OVER_180 )
+	);
+
     glm::vec3 forward(-sinf(getRenderYaw()*PI/180),
                       sinf(getRenderPitch()*PI/180),
                       cosf(getRenderYaw()*PI/180));
     
     thrust = glm::vec3(0);
-    const float THRUST_MAG = 10.0;
+	*/
-    const float THRUST_LATERAL_MAG = 10.0;
+	
-    const float THRUST_VERTICAL_MAG = 10.0;
+    const float THRUST_MAG	= 10.0;
-    
+    const float YAW_MAG		= 300.0;
-    if (driveKeys[FWD]) {
+	
-        thrust += THRUST_MAG*forward;
+	avatar.thrust = glm::vec3( 0.0, 0.0, 0.0 );
+		
+	//notice that the z values from avatar.orientation are flipped to accommodate different coordinate system
+    if (driveKeys[FWD]) 
+	{
+		glm::vec3 front( avatar.orientation.getFront().getX(), avatar.orientation.getFront().getY(), -avatar.orientation.getFront().getZ() );
+		avatar.thrust += front * THRUST_MAG;
     }
-    if (driveKeys[BACK]) {
+    if (driveKeys[BACK]) 
-        thrust += -THRUST_MAG*forward;
+	{
+		glm::vec3 front( avatar.orientation.getFront().getX(), avatar.orientation.getFront().getY(), -avatar.orientation.getFront().getZ() );
+		avatar.thrust -= front * THRUST_MAG;
     }
-    if (driveKeys[RIGHT]) {
+    if (driveKeys[RIGHT]) 
-        thrust.x += forward.z*-THRUST_LATERAL_MAG;
+	{
-        thrust.z += forward.x*THRUST_LATERAL_MAG;
+		glm::vec3 right( avatar.orientation.getRight().getX(), avatar.orientation.getRight().getY(), -avatar.orientation.getRight().getZ() );
+		avatar.thrust += right * THRUST_MAG;
     }
-    if (driveKeys[LEFT]) {
+    if (driveKeys[LEFT]) 
-        thrust.x += forward.z*THRUST_LATERAL_MAG;
+	{
-        thrust.z += forward.x*-THRUST_LATERAL_MAG;
+		glm::vec3 right( avatar.orientation.getRight().getX(), avatar.orientation.getRight().getY(), -avatar.orientation.getRight().getZ() );
+		avatar.thrust -= right * THRUST_MAG;
     }
-    if (driveKeys[UP]) {
+    if (driveKeys[UP])
-        thrust.y += -THRUST_VERTICAL_MAG;
+	{
+		glm::vec3 up( avatar.orientation.getUp().getX(), avatar.orientation.getUp().getY(), -avatar.orientation.getUp().getZ() );
+		avatar.thrust += up * THRUST_MAG;
     }
-    if (driveKeys[DOWN]) {
+    if (driveKeys[DOWN]) 
-        thrust.y += THRUST_VERTICAL_MAG;
+	{
+		glm::vec3 up( avatar.orientation.getUp().getX(), avatar.orientation.getUp().getY(), -avatar.orientation.getUp().getZ() );
+		avatar.thrust -= up * THRUST_MAG;
     }
+	
+    if (driveKeys[ROT_RIGHT]) 
+	{	
+		avatar.yawDelta -= YAW_MAG * deltaTime;
+    }
+    if (driveKeys[ROT_LEFT]) 
+	{	
+		avatar.yawDelta += YAW_MAG * deltaTime;
+    }
+	
+	avatar.yaw += avatar.yawDelta * deltaTime;
+	
+	Yaw = avatar.yaw;
+	
+    const float TEST_YAW_DECAY = 5.0;
+    avatar.yawDelta *= ( 1.0 - TEST_YAW_DECAY * deltaTime );
 
+	//avatar.yawDelta *= 0.99;
+	
+	avatar.velocity += glm::dvec3( avatar.thrust * deltaTime );
+		
+	position += (glm::vec3)avatar.velocity * deltaTime;
+	
+	//avatar.velocity *= 0.9;
+
+    const float LIN_VEL_DECAY = 5.0;
+    avatar.velocity *= ( 1.0 - LIN_VEL_DECAY * deltaTime );
+	
+	
+	/*
     //  Increment velocity as time
     velocity += thrust * deltaTime;
 
     //  Increment position as a function of velocity
     position += velocity * deltaTime;
-    
+    */
+	
+	
+	/*
     //  Decay velocity
     const float LIN_VEL_DECAY = 5.0;
     velocity *= (1.0 - LIN_VEL_DECAY*deltaTime);
-    
+	*/
+	
+	
     if (!noise)
     {
         //  Decay back toward center 
@@ -276,6 +392,8 @@ void Head::simulate(float deltaTime)
     leanForward *= (1.f - DECAY*30.f*deltaTime);
     leanSideways *= (1.f - DECAY*30.f*deltaTime);
     
+	
+	
     //  Update where the avatar's eyes are 
     //
     //  First, decide if we are making eye contact or not
@@ -292,6 +410,8 @@ void Head::simulate(float deltaTime)
         }
     }
     
+	
+	
     const float DEGREES_BETWEEN_VIEWER_EYES = 3;
     const float DEGREES_TO_VIEWER_MOUTH = 7;
 
@@ -315,8 +435,8 @@ void Head::simulate(float deltaTime)
         EyeballPitch[0] = EyeballPitch[1] = -Pitch + eye_target_pitch_adjust;
         EyeballYaw[0] = EyeballYaw[1] = -Yaw + eye_target_yaw_adjust;
     }
+	
 
-    
     if (noise)
     {
         Pitch += (randFloat() - 0.5)*0.2*NoiseEnvelope;
@@ -350,44 +470,71 @@ void Head::simulate(float deltaTime)
         }
                          
     }
-    hand->simulate(deltaTime);
+	
-                         
+	//hand->simulate(deltaTime);
-                         
 }
       
+	  
+	  
+	  
+	   
+//---------------------------------------------------
 void Head::render(int faceToFace, int isMine)
+{
+	renderBody();
+	renderHead( faceToFace, isMine );
+}
+
+
+	  
+	   
+//---------------------------------------------------
+void Head::renderHead( int faceToFace, int isMine )
 {
     int side = 0;
         
     //  Always render own hand, but don't render head unless showing face2face
     glEnable(GL_DEPTH_TEST);
+    glEnable(GL_RESCALE_NORMAL);
+    
     glPushMatrix();
     
-    glScalef(scale, scale, scale);
+	//glScalef(scale, scale, scale);
-    glTranslatef(leanSideways, 0.f, leanForward);
+
+	glTranslatef
+	( 
+		avatar.bone[ AVATAR_BONE_HEAD ].position.x, 
+		avatar.bone[ AVATAR_BONE_HEAD ].position.y, 
+		avatar.bone[ AVATAR_BONE_HEAD ].position.z 
+	);
+
+	glScalef( 0.03, 0.03, 0.03 );
+
+
+	//glTranslatef(leanSideways, 0.f, leanForward);
     
-    glRotatef(Yaw, 0, 1, 0);
+	//glRotatef(Yaw, 0, 1, 0);
+
+    glRotatef( avatar.yaw, 0, 1, 0);
     
-    hand->render(1);
+	//hand->render(1);
     
     //  Don't render a head if it is really close to your location, because that is your own head!
-    if (!isMine || faceToFace) {
+	//if (!isMine || faceToFace) 
+	{
         
         glRotatef(Pitch, 1, 0, 0);
         glRotatef(Roll, 0, 0, 1);
         
-        
         // Overall scale of head
-        if (faceToFace) glScalef(1.5, 2.0, 2.0);
+        if (faceToFace) glScalef(2.0, 2.0, 2.0);
         else glScalef(0.75, 1.0, 1.0);
+		
         glColor3fv(skinColor);
 
-
         //  Head
         glutSolidSphere(1, 30, 30);
-        
+                
-        //std::cout << distanceToCamera << "\n";
-        
         //  Ears
         glPushMatrix();
             glTranslatef(1.0, 0, 0);
@@ -400,7 +547,6 @@ void Head::render(int faceToFace, int isMine)
                 glTranslatef(-2.0, 0, 0);
             }
         glPopMatrix();
-        
 
         // Eyebrows
         audioAttack = 0.9*audioAttack + 0.1*fabs(loudness - lastLoudness);
@@ -509,9 +655,472 @@ void Head::render(int faceToFace, int isMine)
     }
     glPopMatrix();
  }
+ 
+ 
+ 
 
-//  Transmit data to agents requesting it 
+//---------------------------------------------------------
+void Head::setHandMovement( glm::vec3 movement )
+{
+	handOffset = glm::vec3( movement.x, -movement.y, movement.z );
+}
 
+
+
+//-----------------------------------------
+void Head::initializeAvatar()
+{
+	avatar.position		= glm::vec3( 0.0, 0.0, 0.0 );
+	avatar.velocity		= glm::vec3( 0.0, 0.0, 0.0 );
+	avatar.thrust		= glm::vec3( 0.0, 0.0, 0.0 );
+	avatar.orientation.setToIdentity();
+	
+	avatar.yaw		= 90.0;
+	avatar.pitch	= 0.0;
+	avatar.roll		= 0.0;
+	
+	avatar.yawDelta = 0.0;
+	
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		avatar.bone[b].position			= glm::vec3( 0.0, 0.0, 0.0 );
+		avatar.bone[b].springyPosition	= glm::vec3( 0.0, 0.0, 0.0 );
+		avatar.bone[b].springyVelocity	= glm::vec3( 0.0, 0.0, 0.0 );
+		avatar.bone[b].orientation.setToIdentity();
+	}
+
+	//----------------------------------------------------------------------------
+	// parental hierarchy
+	//----------------------------------------------------------------------------
+
+	//----------------------------------------------------------------------------
+	// spine and head
+	//----------------------------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_PELVIS_SPINE		].parent = AVATAR_BONE_NULL;
+	avatar.bone[ AVATAR_BONE_MID_SPINE			].parent = AVATAR_BONE_PELVIS_SPINE;
+	avatar.bone[ AVATAR_BONE_CHEST_SPINE		].parent = AVATAR_BONE_MID_SPINE;
+	avatar.bone[ AVATAR_BONE_NECK				].parent = AVATAR_BONE_CHEST_SPINE;
+	avatar.bone[ AVATAR_BONE_HEAD				].parent = AVATAR_BONE_NECK;
+	
+	//----------------------------------------------------------------------------
+	// left chest and arm
+	//----------------------------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_LEFT_CHEST			].parent = AVATAR_BONE_MID_SPINE;
+	avatar.bone[ AVATAR_BONE_LEFT_SHOULDER		].parent = AVATAR_BONE_LEFT_CHEST;
+	avatar.bone[ AVATAR_BONE_LEFT_UPPER_ARM		].parent = AVATAR_BONE_LEFT_SHOULDER;
+	avatar.bone[ AVATAR_BONE_LEFT_FOREARM		].parent = AVATAR_BONE_LEFT_UPPER_ARM;
+	avatar.bone[ AVATAR_BONE_LEFT_HAND			].parent = AVATAR_BONE_LEFT_FOREARM;
+
+	//----------------------------------------------------------------------------
+	// right chest and arm
+	//----------------------------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_RIGHT_CHEST		].parent = AVATAR_BONE_MID_SPINE;
+	avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER		].parent = AVATAR_BONE_RIGHT_CHEST;
+	avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM	].parent = AVATAR_BONE_RIGHT_SHOULDER;
+	avatar.bone[ AVATAR_BONE_RIGHT_FOREARM		].parent = AVATAR_BONE_RIGHT_UPPER_ARM;
+	avatar.bone[ AVATAR_BONE_RIGHT_HAND			].parent = AVATAR_BONE_RIGHT_FOREARM;
+	
+	//----------------------------------------------------------------------------
+	// left pelvis and leg
+	//----------------------------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_LEFT_PELVIS		].parent = AVATAR_BONE_PELVIS_SPINE;
+	avatar.bone[ AVATAR_BONE_LEFT_THIGH			].parent = AVATAR_BONE_LEFT_PELVIS;
+	avatar.bone[ AVATAR_BONE_LEFT_SHIN			].parent = AVATAR_BONE_LEFT_THIGH;
+	avatar.bone[ AVATAR_BONE_LEFT_FOOT			].parent = AVATAR_BONE_LEFT_SHIN;
+
+	//----------------------------------------------------------------------------
+	// right pelvis and leg
+	//----------------------------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_RIGHT_PELVIS		].parent = AVATAR_BONE_PELVIS_SPINE;
+	avatar.bone[ AVATAR_BONE_RIGHT_THIGH		].parent = AVATAR_BONE_RIGHT_PELVIS;
+	avatar.bone[ AVATAR_BONE_RIGHT_SHIN			].parent = AVATAR_BONE_RIGHT_THIGH;
+	avatar.bone[ AVATAR_BONE_RIGHT_FOOT			].parent = AVATAR_BONE_RIGHT_SHIN;
+
+
+	//----------------------------------------------------------
+	// specify the default pose position
+	//----------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_PELVIS_SPINE		].defaultPosePosition = glm::vec3(  0.0,   0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_MID_SPINE			].defaultPosePosition = glm::vec3(  0.0,   0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_CHEST_SPINE		].defaultPosePosition = glm::vec3(  0.0,   0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_NECK				].defaultPosePosition = glm::vec3(  0.0,   0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_HEAD				].defaultPosePosition = glm::vec3(  0.0,   0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_CHEST			].defaultPosePosition = glm::vec3( -0.06,  0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_SHOULDER		].defaultPosePosition = glm::vec3( -0.03,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_UPPER_ARM		].defaultPosePosition = glm::vec3(  0.0,  -0.12, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_FOREARM		].defaultPosePosition = glm::vec3(  0.0,  -0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_HAND			].defaultPosePosition = glm::vec3(  0.0,  -0.05, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_CHEST		].defaultPosePosition = glm::vec3(  0.06,  0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER		].defaultPosePosition = glm::vec3(  0.03,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM	].defaultPosePosition = glm::vec3(  0.0,  -0.12, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_FOREARM		].defaultPosePosition = glm::vec3(  0.0,  -0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_HAND			].defaultPosePosition = glm::vec3(  0.0,  -0.05, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_PELVIS		].defaultPosePosition = glm::vec3( -0.05,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_THIGH			].defaultPosePosition = glm::vec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_SHIN			].defaultPosePosition = glm::vec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_FOOT			].defaultPosePosition = glm::vec3(  0.0,   0.0,  0.04 );
+	avatar.bone[ AVATAR_BONE_RIGHT_PELVIS		].defaultPosePosition = glm::vec3(  0.05,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_THIGH		].defaultPosePosition = glm::vec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_SHIN			].defaultPosePosition = glm::vec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_FOOT			].defaultPosePosition = glm::vec3(  0.0,   0.0,  0.04 );
+
+	//----------------------------------------------------------------------------
+	// calculate bone length
+	//----------------------------------------------------------------------------
+	calculateBoneLengths();
+
+	//----------------------------------------------------------------------------
+	// generate world positions
+	//----------------------------------------------------------------------------
+	updateAvatarSkeleton();
+	
+
+	
+//avatar.bone[4].springyVelocity = glm::vec3( 1.0f, 0.0f, 0.0f );
+	
+	
+}
+
+
+
+
+//-----------------------------------------
+void Head::calculateBoneLengths()
+{
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		avatar.bone[b].length = glm::length( avatar.bone[b].defaultPosePosition );
+	}
+
+	avatar.maxArmLength
+	= avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM	].length
+	+ avatar.bone[ AVATAR_BONE_RIGHT_FOREARM	].length 
+	+ avatar.bone[ AVATAR_BONE_RIGHT_HAND		].length;
+}
+
+
+//-----------------------------------------
+void Head::updateAvatarSkeleton()
+{
+	//------------------------------------------------------------------------
+	// rotate...
+	//------------------------------------------------------------------------	
+	avatar.orientation.setToIdentity();
+	avatar.orientation.yaw( -avatar.yaw );
+
+	//------------------------------------------------------------------------
+	// calculate positions of all bones by traversing the skeleton tree:
+	//------------------------------------------------------------------------
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		if ( avatar.bone[b].parent == AVATAR_BONE_NULL )
+		{
+			avatar.bone[b].orientation.set( avatar.orientation );
+			avatar.bone[b].position = avatar.position;
+		}
+		else
+		{
+			avatar.bone[b].orientation.set( avatar.bone[ avatar.bone[b].parent ].orientation );
+			avatar.bone[b].position = avatar.bone[ avatar.bone[b].parent ].position;
+		}
+														
+		float xx	= glm::dot( avatar.bone[b].defaultPosePosition.x, (float)avatar.bone[b].orientation.getRight	().x )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.y, (float)avatar.bone[b].orientation.getRight	().y )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.z, (float)avatar.bone[b].orientation.getRight	().z );
+
+		float yy	= glm::dot( avatar.bone[b].defaultPosePosition.x, (float)avatar.bone[b].orientation.getUp		().x )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.y, (float)avatar.bone[b].orientation.getUp		().y )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.z, (float)avatar.bone[b].orientation.getUp		().z );
+
+		float zz	= glm::dot( avatar.bone[b].defaultPosePosition.x, (float)avatar.bone[b].orientation.getFront	().x )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.y, (float)avatar.bone[b].orientation.getFront	().y )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.z, (float)avatar.bone[b].orientation.getFront	().z );
+				
+		glm::vec3 rotatedBoneVector( xx, yy, zz );
+
+		//rotatedBonePosition.x = avatar.bone[b].defaultPosePosition.x;// * avatar.bone[b].orientation.getFront().x;
+		//rotatedBonePosition.y = avatar.bone[b].defaultPosePosition.y;// * avatar.bone[b].orientation.getFront().y;
+		//rotatedBonePosition.z = avatar.bone[b].defaultPosePosition.z;// * avatar.bone[b].orientation.getFront().z;
+	
+
+		
+		//glm::dvec3 rotatedBoneVector( avatar.bone[b].defaultPosePosition );
+
+		//glm::dmat3x3 rotationMatrix ( glm::dvec3( 1.0, 0.0, 0.0 ), glm::dvec3( 0.0, 1.0, 0.0 ), glm::dvec3( 0.0, 0.0, 1.0 ) );
+		//glm::dmat3x3 rotationMatrix;
+		
+		//glm::dmat3x3 rotationMatrix = glm::eulerAngleYXZ( 0.0, 0.0, 0.0 );
+		
+		
+		avatar.bone[b].position += rotatedBoneVector;
+	}	
+	
+
+	//------------------------------------------------------------------------
+	// update springy behavior:
+	//------------------------------------------------------------------------
+	updateAvatarSprings();
+	
+	//------------------------------------------------------------------------
+	// reset hand and elbow position according to hand movement
+	//------------------------------------------------------------------------
+	updateHandMovement();
+	
+	/*
+	glm::dvec3 v( avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position );
+	v -= avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].position;
+	
+	double distance = glm::length(v);
+	if ( distance > avatar.maxArmLength )
+	{
+		avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].position += v * 0.2;
+	}
+	*/
+	
+	
+	
+	/*
+	//------------------------------------------------------------------------
+	// update offset position
+	//------------------------------------------------------------------------
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		glm::dvec3 diff( avatar.bone[b].position );
+		diff -= avatar.bone[b].offsetPosition;
+		
+		avatar.bone[b].offsetPosition += diff * 0.1;
+	}	
+	*/
+	
+	
+}
+
+
+
+
+
+//-------------------------------
+void Head::updateAvatarSprings()
+{
+printf( "listing bone parent springyPosition:\n" );
+
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		printf
+		( 
+			"bone %d: %f, %f, %f\n", b, 
+			avatar.bone[ avatar.bone[b].parent ].springyPosition.x, 
+			avatar.bone[ avatar.bone[b].parent ].springyPosition.y, 
+			avatar.bone[ avatar.bone[b].parent ].springyPosition.z 
+		);
+
+		glm::vec3 springVector( avatar.bone[b].springyPosition );
+
+		if ( avatar.bone[b].parent == AVATAR_BONE_NULL )
+		{
+			springVector -= avatar.position;
+		}
+		else
+		{
+			springVector -= avatar.bone[ avatar.bone[b].parent ].springyPosition;
+			float length = glm::length( springVector );
+			
+			if ( length > 0.0f )
+			{
+				glm::vec3 springDirection = springVector / length;
+				
+				float force = ( length - avatar.bone[b].length ) * 0.01;
+				
+				avatar.bone[ b						].springyVelocity -= springDirection * force;
+				avatar.bone[ avatar.bone[b].parent	].springyVelocity += springDirection * force;
+			}
+			
+			avatar.bone[b].springyVelocity += ( avatar.bone[b].position - avatar.bone[b].springyPosition ) * 0.01f;
+			avatar.bone[b].springyVelocity *= 0.8;
+			avatar.bone[b].springyPosition += avatar.bone[b].springyVelocity;
+		}
+	}
+}
+
+
+//-------------------------------
+float Head::getAvatarYaw()
+{
+	return avatar.yaw;
+}
+
+
+//-------------------------------------------
+glm::vec3 Head::getAvatarHeadLookatDirection()
+{
+	return glm::vec3
+	(
+		avatar.bone[ AVATAR_BONE_HEAD ].orientation.getFront().x,
+		avatar.bone[ AVATAR_BONE_HEAD ].orientation.getFront().y,
+		avatar.bone[ AVATAR_BONE_HEAD ].orientation.getFront().z
+	);
+}
+
+
+
+//-------------------------------
+void Head::updateHandMovement()
+{
+	//----------------------------------------------------------------
+	// adjust right hand and elbow according to hand offset
+	//----------------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position += handOffset;	
+	glm::vec3 armVector = avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position;
+	armVector -= avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].position;
+
+	//-------------------------------------------------------------------------------
+	// test to see if right hand is being dragged beyond maximum arm length
+	//-------------------------------------------------------------------------------
+	float distance = glm::length( armVector );
+	
+	//-------------------------------------------------------------------------------
+	// if right hand is being dragged beyond maximum arm length...
+	//-------------------------------------------------------------------------------
+	if ( distance > avatar.maxArmLength )
+	{
+		//-------------------------------------------------------------------------------
+		// reset right hand to be constrained to maximum arm length
+		//-------------------------------------------------------------------------------
+		avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position = avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].position;
+		glm::vec3 armNormal = armVector / distance;
+		armVector = armNormal * (float)avatar.maxArmLength;
+		distance = avatar.maxArmLength;
+		glm::vec3 constrainedPosition = avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].position;
+		constrainedPosition += armVector;
+		avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position = constrainedPosition;
+	}
+	
+	//-----------------------------------------------------------------------------
+	// set elbow position 
+	//-----------------------------------------------------------------------------
+	glm::vec3 newElbowPosition = avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].position;
+	newElbowPosition += armVector * (float)ONE_HALF;
+	glm::vec3 perpendicular = glm::vec3( -armVector.y, armVector.x, armVector.z );
+	newElbowPosition += perpendicular * (float)( ( 1.0 - ( avatar.maxArmLength / distance ) ) * ONE_HALF );
+	avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].position = newElbowPosition;
+
+	//-----------------------------------------------------------------------------
+	// set wrist position 
+	//-----------------------------------------------------------------------------
+	glm::vec3 vv( avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position );
+	vv -= avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].position;
+	glm::vec3 newWristPosition = avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].position;
+	newWristPosition += vv * 0.7f;
+	avatar.bone[ AVATAR_BONE_RIGHT_FOREARM ].position = newWristPosition;
+}
+
+
+
+
+//-----------------------------------------
+void Head::renderBody()
+{
+	//-----------------------------------------
+    //  Render bone positions as spheres
+	//-----------------------------------------
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		glColor3fv( skinColor );
+		glPushMatrix();
+			glTranslatef( avatar.bone[b].position.x, avatar.bone[b].position.y, avatar.bone[b].position.z );
+			glutSolidSphere( 0.02f, 10.0f, 5.0f );
+		glPopMatrix();
+
+		/*
+		glColor3fv( lightBlue );
+		glPushMatrix();
+			glTranslatef( avatar.bone[b].springyPosition.x, avatar.bone[b].springyPosition.y, avatar.bone[b].springyPosition.z );
+			glutSolidSphere( 0.01f, 10.0f, 5.0f );
+		glPopMatrix();
+		*/
+	}
+	
+
+	
+
+	//-----------------------------------------------------
+    // Render lines connecting the bone positions
+	//-----------------------------------------------------
+    glColor3f(1,1,1);
+    glLineWidth(3.0);
+
+	for (int b=1; b<NUM_AVATAR_BONES; b++)
+	{
+		glBegin( GL_LINE_STRIP );
+		glVertex3fv( &avatar.bone[ avatar.bone[ b ].parent ].position.x);
+		glVertex3fv( &avatar.bone[ b ].position.x);
+		glEnd();
+	}
+
+	/*
+	//-----------------------------------------------------
+    // Render lines connecting the springy positions
+	//-----------------------------------------------------
+    glColor3f( 0.2f, 0.3f, 0.4f );
+    glLineWidth(3.0);
+
+	for (int b=1; b<NUM_AVATAR_BONES; b++)
+	{
+		glBegin( GL_LINE_STRIP );
+		glVertex3fv( &avatar.bone[ avatar.bone[ b ].parent ].springyPosition.x);
+		glVertex3fv( &avatar.bone[ b ].springyPosition.x);
+		glEnd();
+	}
+	*/
+	
+
+	
+	/*
+    glBegin(GL_LINE_STRIP);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_CHEST_SPINE].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_NECK].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_CHEST_SPINE].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_SHOULDER].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_UPPER_ARM].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_FOREARM].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_HAND].position.x);
+    glEnd();
+    glBegin(GL_LINE_STRIP);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_CHEST_SPINE].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_SHOULDER].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_UPPER_ARM].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_FOREARM].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_HAND].position.x);
+    glEnd();
+    glBegin(GL_LINE_STRIP);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_CHEST_SPINE].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_MID_SPINE].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_PELVIS_SPINE].position.x);    
+    glEnd();
+    glBegin(GL_LINE_STRIP);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_PELVIS_SPINE].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_PELVIS].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_THIGH].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_SHIN].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_LEFT_FOOT].position.x);
+    glEnd();
+    glBegin(GL_LINE_STRIP);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_PELVIS_SPINE].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_PELVIS].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_THIGH].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_SHIN].position.x);
+    glVertex3fv(&avatar.bone[AVATAR_BONE_RIGHT_FOOT].position.x);
+    glEnd();
+	*/
+	
+
+}
+
+//  Transmit data to agents requesting it
+
+//called on me just prior to sending data to others (continuasly called)
+
+//---------------------------------------------------
 int Head::getBroadcastData(char* data)
 {
     // Copy data for transmission to the buffer, return length of data
@@ -519,21 +1128,38 @@ int Head::getBroadcastData(char* data)
             getRenderPitch() + Pitch, -getRenderYaw() + 180 -Yaw, Roll,
             position.x + leanSideways, position.y, position.z + leanForward,
             loudness, averageLoudness,
-            hand->getPos().x, hand->getPos().y, hand->getPos().z);
+            //hand->getPos().x, hand->getPos().y, hand->getPos().z);  //previous to Ventrella change
+            avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position.x, 
+			avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position.y, 
+			avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position.z );    // Ventrella change
     return strlen(data);
 }
 
-void Head::parseData(void *data, int size) {
+//called on the other agents - assigns it to my views of the others
+
+//---------------------------------------------------
+void Head::parseData(void *data, int size) 
+{
+	//glm::vec3 pos;//( (glm::vec3)avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position ); 
+	
     // parse head data for this agent
-    glm::vec3 handPos(0,0,0);
+    glm::vec3 handPos( 0,0,0 );
-    sscanf((char *)data, "H%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
+    
-           &Pitch, &Yaw, &Roll,
+	sscanf
-           &position.x, &position.y, &position.z,
+	(
-           &loudness, &averageLoudness,
+		(char *)data, "H%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
-           &handPos.x, &handPos.y, &handPos.z);
+		&Pitch, &Yaw, &Roll,
+		&position.x, &position.y, &position.z,
+		&loudness, &averageLoudness,
+		&avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position.x, 
+		&avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position.y, 
+		&avatar.bone[ AVATAR_BONE_RIGHT_HAND ].position.z
+	);
+		   
     if (glm::length(handPos) > 0.0) hand->setPos(handPos);
 }
 
+//---------------------------------------------------
 void Head::SetNewHeadTarget(float pitch, float yaw)
 {
     PitchTarget = pitch;

interface/src/Head.h

@@ -13,8 +13,9 @@
 #include "AgentData.h"
 #include "Field.h"
 #include "world.h"
-#include "Head.h"
 #include "Hand.h"
+#include "Vector3D.h"		// added by Ventrella as a utility
+#include "Orientation.h"	// added by Ventrella as a utility
 #include "InterfaceConfig.h"
 #include "SerialInterface.h"
 
@@ -30,6 +31,104 @@ enum eyeContactTargets {LEFT_EYE, RIGHT_EYE, MOUTH};
 #define ROT_RIGHT 7 
 #define MAX_DRIVE_KEYS 8
 
+/*
+enum AvatarJoints
+{
+	AVATAR_JOINT_NULL = -1,
+	AVATAR_JOINT_PELVIS,
+	AVATAR_JOINT_TORSO,
+	AVATAR_JOINT_CHEST,
+	AVATAR_JOINT_NECK_BASE,
+	AVATAR_JOINT_HEAD_BASE,
+	AVATAR_JOINT_HEAD_TOP,
+	
+	AVATAR_JOINT_LEFT_CLAVICLE,
+	AVATAR_JOINT_LEFT_SHOULDER,
+	AVATAR_JOINT_LEFT_ELBOW,
+	AVATAR_JOINT_LEFT_WRIST,
+	AVATAR_JOINT_LEFT_FINGERTIPS,
+	
+	AVATAR_JOINT_RIGHT_CLAVICLE,
+	AVATAR_JOINT_RIGHT_SHOULDER,
+	AVATAR_JOINT_RIGHT_ELBOW,
+	AVATAR_JOINT_RIGHT_WRIST,
+	AVATAR_JOINT_RIGHT_FINGERTIPS,
+	
+	AVATAR_JOINT_LEFT_HIP,
+	AVATAR_JOINT_LEFT_KNEE,
+	AVATAR_JOINT_LEFT_HEEL,
+	AVATAR_JOINT_LEFT_TOES,
+	
+	AVATAR_JOINT_RIGHT_HIP,
+	AVATAR_JOINT_RIGHT_KNEE,
+	AVATAR_JOINT_RIGHT_HEEL,
+	AVATAR_JOINT_RIGHT_TOES,
+	
+	NUM_AVATAR_JOINTS
+};
+*/
+
+
+
+
+enum AvatarBones
+{
+	AVATAR_BONE_NULL = -1,
+	AVATAR_BONE_PELVIS_SPINE,		// connects pelvis			joint with torso			joint (not supposed to be rotated)
+	AVATAR_BONE_MID_SPINE,			// connects torso			joint with chest			joint
+	AVATAR_BONE_CHEST_SPINE,		// connects chest			joint with neckBase			joint (not supposed to be rotated)
+	AVATAR_BONE_NECK,				// connects neckBase		joint with headBase			joint
+	AVATAR_BONE_HEAD,				// connects headBase		joint with headTop			joint
+	AVATAR_BONE_LEFT_CHEST,			// connects chest			joint with left clavicle	joint (not supposed to be rotated)
+	AVATAR_BONE_LEFT_SHOULDER,		// connects left clavicle	joint with left shoulder	joint
+	AVATAR_BONE_LEFT_UPPER_ARM,		// connects left shoulder	joint with left elbow		joint
+	AVATAR_BONE_LEFT_FOREARM,		// connects left elbow		joint with left wrist		joint
+	AVATAR_BONE_LEFT_HAND,			// connects left wrist		joint with left fingertips	joint
+	AVATAR_BONE_RIGHT_CHEST,		// connects chest			joint with right clavicle	joint (not supposed to be rotated)
+	AVATAR_BONE_RIGHT_SHOULDER,		// connects right clavicle	joint with right shoulder	joint
+	AVATAR_BONE_RIGHT_UPPER_ARM,	// connects right shoulder	joint with right elbow		joint
+	AVATAR_BONE_RIGHT_FOREARM,		// connects right elbow		joint with right wrist		joint
+	AVATAR_BONE_RIGHT_HAND,			// connects right wrist		joint with right fingertips	joint
+	AVATAR_BONE_LEFT_PELVIS,		// connects pelvis			joint with left hip			joint (not supposed to be rotated)
+	AVATAR_BONE_LEFT_THIGH,			// connects left hip		joint with left knee		joint
+	AVATAR_BONE_LEFT_SHIN,			// connects left knee		joint with left heel		joint
+	AVATAR_BONE_LEFT_FOOT,			// connects left heel		joint with left toes		joint
+	AVATAR_BONE_RIGHT_PELVIS,		// connects pelvis			joint with right hip		joint (not supposed to be rotated)
+	AVATAR_BONE_RIGHT_THIGH,		// connects right hip		joint with right knee		joint
+	AVATAR_BONE_RIGHT_SHIN,			// connects right knee		joint with right heel		joint
+	AVATAR_BONE_RIGHT_FOOT,			// connects right heel		joint with right toes		joint
+
+	NUM_AVATAR_BONES
+};
+
+struct AvatarBone
+{
+	AvatarBones	parent;					// which bone is this bone connected to?
+	glm::vec3	position;				// the position at the "end" of the bone
+	glm::vec3	defaultPosePosition;	// the parent relative position when the avatar is in the "T-pose"
+	glm::vec3	springyPosition;		// used for special effects (a 'flexible' variant of position)
+	glm::dvec3	springyVelocity;		// used for special effects ( the velocity of the springy position)
+	float		yaw;					// the yaw Euler angle of the bone rotation off the parent
+	float		pitch;					// the pitch Euler angle of the bone rotation off the parent
+	float		roll;					// the roll Euler angle of the bone rotation off the parent
+	Orientation	orientation;			// three orthogonal normals determined by yaw, pitch, roll
+	float		length;					// the length of the bone
+};
+
+struct Avatar
+{
+	glm::vec3	position;
+	glm::dvec3	velocity;
+	glm::vec3	thrust;
+	float		yaw;
+	float		pitch;
+	float		roll;
+	float		yawDelta;
+	float		maxArmLength;
+	Orientation	orientation;
+	AvatarBone	bone[ NUM_AVATAR_BONES ];
+};
+
 class Head : public AgentData {
     public:
         Head();
@@ -58,9 +157,20 @@ class Head : public AgentData {
         float getRoll() {return Roll;}
         float getYaw() {return Yaw;}
         float getLastMeasuredYaw() {return YawRate;}
+		
+		float getAvatarYaw();
+		glm::vec3 getAvatarHeadLookatDirection();
         
         void render(int faceToFace, int isMine);
+		
+		void setAvatarPosition( float, float, float );
+		void renderBody();
+		void renderHead( int faceToFace, int isMine );
+
         void simulate(float);
+				
+		void setHandMovement( glm::vec3 movement );
+		void updateHandMovement();
         
         //  Send and receive network data
         int getBroadcastData(char * data);
@@ -124,6 +234,8 @@ class Head : public AgentData {
         glm::vec3 position;
         glm::vec3 velocity;
         glm::vec3 thrust;
+		
+		glm::vec3 handOffset;
     
         int driveKeys[MAX_DRIVE_KEYS];
         
@@ -131,10 +243,16 @@ class Head : public AgentData {
         eyeContactTargets eyeContactTarget;
     
         GLUquadric *sphere;
+		Avatar avatar;
+		
+		void initializeAvatar();
+		void updateAvatarSkeleton();
+		void updateAvatarSprings();
+		void calculateBoneLengths();
     
         void readSensors();
         float renderYaw, renderPitch;       //   Pitch from view frustum when this is own head.
     
 };
 
-#endif
+#endif

interface/src/Orientation.cpp

@@ -0,0 +1,232 @@
+//-----------------------------------------------------------
+//
+// Created by Jeffrey Ventrella and added as a utility 
+// class for High Fidelity Code base, April 2013
+//
+//-----------------------------------------------------------
+
+#include "Orientation.h"
+#include "Vector3D.h"
+#include "Util.h"
+
+//------------------------
+Orientation::Orientation()
+{
+	right.setXYZ	(  1.0,  0.0,  0.0 );
+	up.setXYZ		(  0.0,  1.0,  0.0 );
+	front.setXYZ	(  0.0,  0.0,  1.0 );	
+}
+
+
+//--------------------------------
+void Orientation::setToIdentity()
+{
+	right.setXYZ	(  1.0,  0.0,  0.0 );
+	up.setXYZ		(  0.0,  1.0,  0.0 );
+	front.setXYZ	(  0.0,  0.0,  1.0 );	
+}
+
+
+//------------------------------------
+void Orientation::set( Orientation o )
+{ 
+	right.set	( o.getRight() );
+	up.set		( o.getUp	() );
+	front.set	( o.getFront() );	
+}
+
+
+//-----------------------------------------------------------------------------------------
+void Orientation::forceAxisInDirection( int whichAxis, const Vector3D &direction, double forceAmount )
+{
+	Vector3D diff;
+	
+	if ( whichAxis == ORIENTATION_RIGHT_AXIS )
+	{
+		diff.setToDifference( direction, right );
+		right.addScaled( diff, forceAmount );
+		right.normalize();
+		up.setToCross( front, right );
+		up.normalize();
+		front.setToCross( right, up );
+	}
+	else if ( whichAxis == ORIENTATION_UP_AXIS )
+	{
+		diff.setToDifference( direction, up );
+		up.addScaled( diff, forceAmount );
+		up.normalize();
+		front.setToCross( right, up );
+		front.normalize();
+		right.setToCross( up, front );
+	}
+	else if ( whichAxis == ORIENTATION_FRONT_AXIS )
+	{
+		diff.setToDifference( direction, front );
+		front.addScaled( diff, forceAmount );
+		front.normalize();
+		right.setToCross( up, front );
+		right.normalize();
+		up.setToCross( front, right );
+	}	
+}
+
+
+//------------------------------------------------------------------------------------------------------
+void Orientation::forceFrontInDirection( const Vector3D &direction, const Vector3D &upDirection, double forceAmount )
+{
+	Vector3D diff;
+	diff.setToDifference( direction, front );
+	front.addScaled( diff, forceAmount );
+	front.normalize();	
+	right.setToCross( upDirection, front );
+	right.normalize();
+	up.setToCross( front, right );	
+}
+
+
+
+
+//---------------------------------------
+void Orientation::yaw( double angle )
+{
+	double r = angle * PI_OVER_180;
+	double s = sin( r );
+	double c = cos( r );
+	
+	Vector3D cosineFront;
+	Vector3D cosineRight;
+	Vector3D sineFront;
+	Vector3D sineRight;
+	
+	cosineFront.setToScaled	( front, c );
+	cosineRight.setToScaled	( right, c );
+	sineFront.setToScaled	( front, s );
+	sineRight.setToScaled	( right, s );
+	
+	front.set( cosineFront	);
+	front.add( sineRight	);
+	
+	right.set( cosineRight		);
+	right.subtract( sineFront	);	
+}
+
+
+//---------------------------------------
+void Orientation::pitch( double angle )
+{
+	double r = angle * PI_OVER_180;
+	double s = sin( r );
+	double c = cos( r );
+
+	Vector3D cosineUp;
+	Vector3D cosineFront;
+	Vector3D sineUp;
+	Vector3D sineFront;
+	
+	cosineUp.setToScaled	( up,    c );
+	cosineFront.setToScaled	( front, c );
+	sineUp.setToScaled		( up,    s );
+	sineFront.setToScaled	( front, s );
+	
+	up.set( cosineUp );
+	up.add( sineFront );
+	
+	front.set( cosineFront );
+	front.subtract( sineUp  );
+}
+
+
+//---------------------------------------
+void Orientation::roll( double angle )
+{
+	double r = angle * PI_OVER_180;
+	double s = sin( r );
+	double c = cos( r );
+	
+	Vector3D cosineUp;
+	Vector3D cosineRight;
+	Vector3D sineUp;
+	Vector3D sineRight;
+	
+	cosineUp.setToScaled	( up,    c );
+	cosineRight.setToScaled	( right, c );
+	sineUp.setToScaled		( up,    s );
+	sineRight.setToScaled	( right, s );
+	
+	up.set( cosineUp );
+	up.add( sineRight );
+	
+	right.set( cosineRight );
+	right.subtract( sineUp	);	
+}
+
+
+Vector3D Orientation::getRight	() { return right;	}
+Vector3D Orientation::getUp		() { return up;		}
+Vector3D Orientation::getFront	() { return front;	}
+
+
+//-----------------------------------------------------------------------------
+void Orientation::setRightUpFront( const Vector3D &r, const Vector3D &u, const Vector3D &f )
+{
+	//verifyValidOrientation();
+
+	right.set	(r);
+	up.set		(u);
+	front.set	(f);
+}
+
+
+//-----------------------------------------------------------------------------
+void Orientation::verifyValidOrientation()
+{
+	assert( right.getMagnitude	() < 1.0 + CENTIMETER );
+	assert( right.getMagnitude	() > 1.0 - CENTIMETER );
+	assert( up.getMagnitude		() < 1.0 + CENTIMETER );
+	assert( up.getMagnitude		() > 1.0 - CENTIMETER );
+	assert( front.getMagnitude	() < 1.0 + CENTIMETER );
+	assert( front.getMagnitude	() > 1.0 - CENTIMETER );
+	
+	if ( right.getMagnitude() > 1.0 + CENTIMETER ) 
+	{ 
+		printf( "oops: the magnitude of the 'right' part of the orientation is %f!\n", right.getMagnitude() ); 
+	}
+	else if ( right.getMagnitude() < 1.0 - CENTIMETER ) 
+	{ 
+		printf( "oops: the magnitude of the 'right' part of the orientation is %f!\n", right.getMagnitude() ); 
+	}
+
+
+	if ( up.getMagnitude() > 1.0 + CENTIMETER ) 
+	{ 
+		printf( "oops: the magnitude of the 'up' part of the orientation is %f!\n", up.getMagnitude() ); 
+	}
+	else if ( up.getMagnitude() < 1.0 - CENTIMETER ) 
+	{ 
+		printf( "oops: the magnitude of the 'up' part of the orientation is %f!\n", up.getMagnitude() ); 
+	}
+
+
+	if ( front.getMagnitude() > 1.0 + CENTIMETER ) 
+	{ 
+		printf( "oops: the magnitude of the 'front' part of the orientation is %f!\n", front.getMagnitude() ); 
+	}
+	else if ( front.getMagnitude() < 1.0 - CENTIMETER ) 
+	{ 
+		printf( "oops: the magnitude of the 'front' part of the orientation is %f!\n", front.getMagnitude() ); 
+	}
+	
+	if (( right.dotWith	( up	) >  CENTIMETER )
+	||  ( right.dotWith	( up	) < -CENTIMETER )) { printf( "oops: the 'right' and 'up' parts of the orientation are not perpendicular! The dot is: %f\n", right.dotWith	( up	)	); }
+	
+	if (( right.dotWith	( front	) >  CENTIMETER )
+	||  ( right.dotWith	( front	) < -CENTIMETER )) { printf( "oops: the 'right' and 'front' parts of the orientation are not perpendicular! The dot is: %f\n",	right.dotWith	( front	) ); }
+	
+	if (( up.dotWith	( front	) >  CENTIMETER )
+	||  ( up.dotWith	( front	) < -CENTIMETER )) { printf( "oops: the 'up' and 'front' parts of the orientation are not perpendicular! The dot is: %f\n", up.dotWith	( front	)	); }
+	
+}
+
+
+
+

interface/src/Orientation.h

@@ -0,0 +1,52 @@
+//-----------------------------------------------------------
+//
+// Created by Jeffrey Ventrella and added as a utility 
+// class for High Fidelity Code base, April 2013
+//
+//-----------------------------------------------------------
+
+#ifndef __interface__orientation__
+#define __interface__orientation__
+
+#include <cmath> // with this work? "Math.h"
+#include "Vector3D.h"
+
+enum Axis
+{
+	ORIENTATION_RIGHT_AXIS,
+	ORIENTATION_UP_AXIS,
+	ORIENTATION_FRONT_AXIS
+};
+
+class Orientation
+{
+private:
+	
+	Vector3D right;
+	Vector3D up;
+	Vector3D front;
+	
+	void verifyValidOrientation();
+	
+public:
+	Orientation();
+	
+	void yaw	( double );
+	void pitch	( double );
+	void roll	( double );
+
+	void set( Orientation );
+	void setToIdentity();
+
+	void forceFrontInDirection( const Vector3D &, const Vector3D &, double );
+	void forceAxisInDirection( int, const Vector3D &, double );
+
+	Vector3D getRight();
+	Vector3D getUp();
+	Vector3D getFront();
+	
+	void setRightUpFront( const Vector3D &, const Vector3D &, const Vector3D & );
+};
+
+
+#endif

interface/src/Util.cpp

@@ -140,3 +140,26 @@ void drawvec3(int x, int y, float scale, float rotate, float thick, int mono, gl
     glPopMatrix();
     
 } 
+
+// XXXBHG - These handy operators should probably go somewhere else, I'm surprised they don't
+// already exist somewhere in OpenGL. Maybe someone can point me to them if they do exist!
+glm::vec3 operator* (float lhs, const glm::vec3& rhs)
+{
+    glm::vec3 result = rhs;
+    result.x *= lhs;
+    result.y *= lhs;
+    result.z *= lhs;
+    return result;
+}
+
+// XXXBHG - These handy operators should probably go somewhere else, I'm surprised they don't
+// already exist somewhere in OpenGL. Maybe someone can point me to them if they do exist!
+glm::vec3 operator* (const glm::vec3& lhs, float rhs)
+{
+    glm::vec3 result = lhs;
+    result.x *= rhs;
+    result.y *= rhs;
+    result.z *= rhs;
+    return result;
+}
+

interface/src/Util.h

@@ -17,6 +17,24 @@
 
 #include <glm/glm.hpp>
 
+
+// added by Ventrella for utility purposes
+static const double	ZERO				= 0.0;
+static const double	ONE					= 1.0;
+static const double	ONE_HALF			= 0.5;
+static const double	ONE_THIRD			= 0.3333333;
+static const double	PIE					= 3.14159265359;
+static const double	PI_TIMES_TWO		= 3.14159265359 * 2.0;
+static const double PI_OVER_180			= 3.14159265359 / 180.0;
+static const double EPSILON				= 0.00001;	//smallish number - used as margin of error for some values 
+static const double SQUARE_ROOT_OF_2	= sqrt(2);	
+static const double SQUARE_ROOT_OF_3	= sqrt(3);	
+
+static const double METER		= 1.0; 
+static const double DECIMETER	= 0.1; 
+static const double CENTIMETER	= 0.01; 
+static const double MILLIIMETER	= 0.001; 
+
 float azimuth_to(glm::vec3 head_pos, glm::vec3 source_pos);
 float angle_to(glm::vec3 head_pos, glm::vec3 source_pos, float render_yaw, float head_yaw);
 
@@ -30,4 +48,8 @@ void drawvec3(int x, int y, float scale, float rotate, float thick, int mono, gl
 double diffclock(timeval *clock1,timeval *clock2);
 
 
+glm::vec3 operator* (float lhs, const glm::vec3& rhs);
+glm::vec3 operator* (const glm::vec3& lhs, float rhs);
+
+
 #endif

interface/src/Vector3D.cpp

@@ -0,0 +1,267 @@
+//-----------------------------------------------------------
+//
+// Created by Jeffrey Ventrella and added as a utility 
+// class for High Fidelity Code base, April 2013
+//
+//-----------------------------------------------------------
+
+#include "Vector3D.h"
+#include <cmath> // "Math.h"
+
+//---------------------------------------
+Vector3D::Vector3D()
+{
+	x = 0.0;
+	y = 0.0;
+	z = 0.0;	
+}
+
+//---------------------------------------
+Vector3D::Vector3D( double x_, double y_, double z_ )
+{
+	x = x_;
+	y = y_;
+	z = z_;	
+}
+
+//---------------------------------------
+Vector3D::Vector3D( const Vector3D & v )
+{
+	x = v.x;
+	y = v.y;
+	z = v.z;
+}
+
+//-----------------------------------------------------
+void Vector3D::setXYZ( double x_, double y_, double z_ )
+{
+	x = x_;
+	y = y_;
+	z = z_;	
+}
+
+
+//---------------------
+void Vector3D::clear()
+{
+	x = 0.0;
+	y = 0.0;
+	z = 0.0;	
+}
+
+
+//-----------------------------------------------------
+void Vector3D::addXYZ( double x_, double y_, double z_ )
+{
+	x += x_;
+	y += y_;
+	z += z_;	
+}
+
+//---------------------------------------
+void Vector3D::set( const Vector3D &v )
+{
+	x = v.x;
+	y = v.y;
+	z = v.z;	
+}
+
+//-------------------------------------
+void Vector3D::add( const Vector3D &v )
+{
+	x += v.x;
+	y += v.y;
+	z += v.z;	
+}
+
+
+//--------------------------------------------
+void Vector3D::subtract ( const Vector3D &v )
+{
+	x -= v.x;
+	y -= v.y;
+	z -= v.z;	
+}
+
+//-----------------------------------------------------
+void Vector3D::addScaled( const Vector3D &v, double s )
+{
+	x += v.x * s;
+	y += v.y * s;
+	z += v.z * s;	
+}
+
+//-----------------------------------------------------
+void Vector3D::subtractScaled( const Vector3D &v, double s )
+{
+	x -= v.x * s;
+	y -= v.y * s;
+	z -= v.z * s;	
+}
+	   
+
+//-------------------------
+void Vector3D::normalize()
+{
+	double d = sqrt( x * x + y * y + z * z );
+	
+	if ( d > 0.0 )
+	{
+		x /= d;
+		y /= d;
+		z /= d;
+	}	
+}
+
+
+//--------------------------------------------
+void Vector3D::setX ( double x_ ) { x = x_; }
+void Vector3D::setY ( double y_ ) { y = y_; }
+void Vector3D::setZ ( double z_ ) { z = z_; }
+
+void Vector3D::addX ( double x_ ) { x += x_; }
+void Vector3D::addY ( double y_ ) { y += y_; }
+void Vector3D::addZ ( double z_ ) { z += z_; }
+
+double Vector3D::getX () { return x; }
+double Vector3D::getY () { return y; }
+double Vector3D::getZ () { return z; }
+
+void Vector3D::scaleX ( double s ) { x *= s; }
+void Vector3D::scaleY ( double s ) { y *= s; }
+void Vector3D::scaleZ ( double s ) { z *= s; }
+
+
+//-----------------------------------------------------
+void Vector3D::setToScaled( const Vector3D &v, double s )
+{
+	Vector3D c;
+	
+	x = v.x * s;
+	y = v.y * s;
+	z = v.z * s;	
+}
+
+//--------------------------------------------------------------------
+void Vector3D::setToAverage( const Vector3D &v1, const Vector3D &v2 )
+{
+	x = v1.x + ( v2.x - v1.x ) * 0.5;
+	y = v1.y + ( v2.y - v1.y ) * 0.5;
+	z = v1.z + ( v2.z - v1.z ) * 0.5;	
+}
+
+
+//-----------------------------------------------------
+void Vector3D::setToDifference( const Vector3D &v1, const Vector3D &v2 )
+{
+	x = v1.x - v2.x;
+	y = v1.y - v2.y;
+	z = v1.z - v2.z;	
+}
+
+//-----------------------------------------------------
+void Vector3D::scale( double s )
+{
+	x *= s;
+	y *= s;
+	z *= s;	
+}
+
+//-----------------------------------------------------
+double Vector3D::getMagnitude()
+{	
+	return sqrt( x * x + y * y + z * z );	
+}
+
+
+//-----------------------------------------------------
+double Vector3D::getMagnitudeSquared()
+{	
+	return x * x + y * y + z * z ;	
+}
+
+//-----------------------------------------------------
+double Vector3D::getDistanceTo( const Vector3D &v )
+{
+	double xx = v.x - x;
+	double yy = v.y - y;
+	double zz = v.z - z;
+	
+	return sqrt( xx * xx + yy * yy + zz * zz );
+}
+
+
+//-----------------------------------------------------
+double Vector3D::getDistanceSquaredTo( const Vector3D &v )
+{
+	double xx = v.x - x;
+	double yy = v.y - y;
+	double zz = v.z - z;
+	
+	return xx * xx + yy * yy + zz * zz;
+}
+
+
+//-------------------------------------------------------------------
+double Vector3D::getDistance( const Vector3D &v1, const Vector3D &v2 )
+{	
+	double xx = v2.x - v1.x;
+	double yy = v2.y - v1.y;
+	double zz = v2.z - v1.z;
+	
+	return sqrt( xx * xx + yy * yy + zz * zz );	
+}
+
+
+//---------------------------------------------------------------------------
+double Vector3D::getDistanceSquared( const Vector3D &v1, const Vector3D &v2 )
+{	
+	double xx = v2.x - v1.x;
+	double yy = v2.y - v1.y;
+	double zz = v2.z - v1.z;
+	
+	return xx * xx + yy * yy + zz * zz;	
+}
+
+
+//-----------------------------------------------------
+double Vector3D::dotWith( const Vector3D &v )
+{	
+	return 
+	x * v.x +
+	y * v.y +
+	z * v.z;	
+}
+
+
+
+//-----------------------------------------------------------------
+void Vector3D::setToCross( const Vector3D &v1, const Vector3D &v2 )
+{	
+	x = v1.z * v2.y - v1.y * v2.z;
+	y = v1.x * v2.z - v1.z * v2.x;
+	z = v1.y * v2.x - v1.x * v2.y;	
+}
+
+
+//---------------------------------------------------------------
+void Vector3D::setToSum( const Vector3D &v1, const Vector3D &v2 )
+{	
+	x = v1.x + v2.x;
+	y = v1.y + v2.y;
+	z = v1.z + v2.z;	
+}
+
+
+//-----------------------------------------------------
+void Vector3D::halve()
+{
+	x *= 0.5;
+	y *= 0.5;
+	z *= 0.5;
+}
+
+
+
+
+

interface/src/Vector3D.h

@@ -0,0 +1,66 @@
+//-----------------------------------------------------------
+//
+// Created by Jeffrey Ventrella and added as a utility 
+// class for High Fidelity Code base, April 2013
+//
+//-----------------------------------------------------------
+
+#ifndef __interface__vector3D__
+#define __interface__vector3D__
+
+class Vector3D
+{
+public:
+
+	//------------------
+	// members
+	//------------------
+	double x;
+	double y;
+	double z;
+
+	//------------------
+	// methods
+	//------------------
+	Vector3D();
+	Vector3D( double, double, double );
+	Vector3D( const Vector3D & );
+	
+	void	clear();
+	void	set				( const Vector3D & );
+	void	setToScaled		( const Vector3D &, double );
+	void	add				( const Vector3D & );
+	void	subtract		( const Vector3D & );
+	void	addScaled		( const Vector3D &, double );
+	void	subtractScaled	( const Vector3D &, double );
+	void	normalize		();
+	void	setToCross		( const Vector3D &, const Vector3D & );
+	void	setToAverage	( const Vector3D &, const Vector3D & );
+	void	setToSum		( const Vector3D &, const Vector3D & );
+	void	setXYZ			( double, double, double );
+	void	addXYZ			( double, double, double );
+	void	setX			( double );
+	void	setY			( double );
+	void	setZ			( double );
+	void	addX			( double );
+	void	addY			( double );
+	void	addZ			( double );
+	void	scaleX			( double );
+	void	scaleY			( double );
+	void	scaleZ			( double );
+	void	halve			();
+	double	getX			();
+	double	getY			();
+	double	getZ			();
+	double	getMagnitude	();
+	double	getMagnitudeSquared	();
+	double	getDistance			( const Vector3D &, const Vector3D & );
+	double	getDistanceSquared	( const Vector3D &, const Vector3D & );
+	double	getDistanceTo		( const Vector3D & );
+	double	getDistanceSquaredTo( const Vector3D & );
+	double	dotWith				( const Vector3D & );
+	void	scale				( double );
+	void	setToDifference		( const Vector3D &, const Vector3D & );
+};
+
+#endif 

interface/src/VoxelSystem.cpp

@@ -11,6 +11,7 @@
 #include <iostream> // to load voxels from file
 #include <fstream> // to load voxels from file
 #include <SharedUtil.h>
+#include <PacketHeaders.h>
 #include <OctalCode.h>
 #include "VoxelSystem.h"
 
@@ -113,15 +114,15 @@ void VoxelSystem::parseData(void *data, int size) {
     unsigned char *voxelData = (unsigned char *) data + 1;
     
     switch(command) {
-        case 'V':
+        case PACKET_HEADER_VOXEL_DATA:
             // ask the VoxelTree to read the bitstream into the tree
             tree->readBitstreamToTree(voxelData, size - 1);
         break;
-        case 'R':
+        case PACKET_HEADER_ERASE_VOXEL:
             // ask the tree to read the "remove" bitstream
             tree->processRemoveVoxelBitstream((unsigned char*)data,size);
         break;
-        case 'Z':
+        case PACKET_HEADER_Z_COMMAND:
 
             // the Z command is a special command that allows the sender to send high level semantic
             // requests, like erase all, or add sphere scene, different receivers may handle these 

interface/src/octal.cpp

@@ -1,29 +0,0 @@
-//
-//  octal.cpp
-//  interface
-//
-//  Created by Philip on 2/4/13.
-//  Copyright (c) 2013 HighFidelity, Inc. All rights reserved.
-//
-//  Various subroutines for converting between X,Y,Z coords and octree coordinates.
-//  
-
-#include "Util.h"
-#include "octal.h"
-#include <cstring>
-
-const int X = 0;
-const int Y = 1;
-const int Z = 2;
-
-domainNode rootNode; 
-
-//  Given a position vector between zero and one (but less than one), and a voxel scale 1/2^scale,
-//  returns the smallest voxel at that scale which encloses the given point.
-void getVoxel(float * pos, int scale, float * vpos) {
-    float vscale = powf(2, scale);
-    vpos[X] = floor(pos[X]*vscale)/vscale;
-    vpos[Y] = floor(pos[Y]*vscale)/vscale;
-    vpos[Z] = floor(pos[Z]*vscale)/vscale;
-}
-

interface/src/octal.h

@@ -1,26 +0,0 @@
-//
-//  octal.h
-//  interface
-//
-//  Created by Philip on 2/4/13.
-//  Copyright (c) 2013 HighFidelity, Inc. All rights reserved.
-//
-
-#ifndef __interface__octal__
-#define __interface__octal__
-
-#include <iostream>
-
-struct domainNode {
-    domainNode * child[8];
-    char * hostname;
-    char * nickname;
-    int domain_id;
-};
-
-domainNode* createNode(int lengthInBits, char * octalData,
-                       char * hostname, char * nickname, int domain_id);
-
-
-
-#endif /* defined(__interface__octal__) */

interface/src/starfield/Loader.h

@@ -50,7 +50,7 @@ namespace starfield {
 
                 return false;
             }
- fprintf(stderr, "Stars.cpp: read %d vertices, using %d\n", 
+ fprintf(stderr, "Stars.cpp: read %d stars, rendering %d\n", 
       _valRecordsRead, _ptrVertices->size());
 
             return true;

mixer/src/main.cpp

@@ -17,9 +17,11 @@
 #include <fstream>
 #include <limits>
 #include <AgentList.h>
+#include <AgentTypes.h>
 #include <SharedUtil.h>
 #include <StdDev.h>
 #include "AudioRingBuffer.h"
+#include "PacketHeaders.h"
 
 #ifdef _WIN32
 #include "Syssocket.h"
@@ -59,7 +61,7 @@ const int AGENT_LOOPBACK_MODIFIER = 307;
 
 const int LOOPBACK_SANITY_CHECK = 0;
 
-AgentList agentList('M', MIXER_LISTEN_PORT);
+AgentList agentList(AGENT_TYPE_MIXER, MIXER_LISTEN_PORT);
 StDev stdev;
 
 void plateauAdditionOfSamples(int16_t &mixSample, int16_t sampleToAdd) {
@@ -275,7 +277,7 @@ int main(int argc, const char * argv[])
 
     while (true) {
         if(agentList.getAgentSocket().receive(agentAddress, packetData, &receivedBytes)) {
-            if (packetData[0] == 'I') {
+            if (packetData[0] == PACKET_HEADER_INJECT_AUDIO) {
                                 
                 //  Compute and report standard deviation for jitter calculation
                 if (firstSample) {
@@ -286,7 +288,7 @@ int main(int argc, const char * argv[])
                     stdev.addValue(tDiff);
                     
                     if (stdev.getSamples() > 500) {
-                        printf("Avg: %4.2f, Stdev: %4.2f\n", stdev.getAverage(), stdev.getStDev());
+                        //printf("Avg: %4.2f, Stdev: %4.2f\n", stdev.getAverage(), stdev.getStDev());
                         stdev.reset();
                     }
                 }

shared/src/Agent.cpp

@@ -7,6 +7,7 @@
 //
 
 #include "Agent.h"
+#include "AgentTypes.h"
 #include <cstring>
 #include "UDPSocket.h"
 #include "SharedUtil.h"
@@ -90,10 +91,36 @@ Agent::~Agent() {
     delete linkedData;
 }
 
-char Agent::getType() {
+char Agent::getType() const {
     return type;
 }
 
+// Names of Agent Types
+const char* AGENT_TYPE_NAME_DOMAIN = "Domain";
+const char* AGENT_TYPE_NAME_VOXEL = "Voxel Server";
+const char* AGENT_TYPE_NAME_INTERFACE = "Client Interface";
+const char* AGENT_TYPE_NAME_MIXER = "Audio Mixer";
+const char* AGENT_TYPE_NAME_UNKNOWN = "Unknown";
+
+const char* Agent::getTypeName() const {
+	const char* name = AGENT_TYPE_NAME_UNKNOWN;
+	switch (this->type) {
+		case AGENT_TYPE_DOMAIN:
+			name = AGENT_TYPE_NAME_DOMAIN;
+			break;
+		case AGENT_TYPE_VOXEL:
+			name = AGENT_TYPE_NAME_VOXEL;
+			break;
+		case AGENT_TYPE_INTERFACE:
+			name = AGENT_TYPE_NAME_INTERFACE;
+			break;
+		case AGENT_TYPE_MIXER:
+			name = AGENT_TYPE_NAME_MIXER;
+			break;
+	}
+	return name;
+}
+
 void Agent::setType(char newType) {
     type = newType;
 }
@@ -174,7 +201,7 @@ std::ostream& operator<<(std::ostream& os, const Agent* agent) {
     sockaddr_in *agentPublicSocket = (sockaddr_in *)agent->publicSocket;
     sockaddr_in *agentLocalSocket = (sockaddr_in *)agent->localSocket;
     
-    os << "T: " << agent->type << " PA: " << inet_ntoa(agentPublicSocket->sin_addr) <<
+    os << "T: " << agent->getTypeName() << " (" << agent->type << ") PA: " << inet_ntoa(agentPublicSocket->sin_addr) <<
         ":" << ntohs(agentPublicSocket->sin_port) << " LA: " << inet_ntoa(agentLocalSocket->sin_addr) <<
         ":" << ntohs(agentLocalSocket->sin_port);
     return os;

shared/src/Agent.h

@@ -40,7 +40,8 @@ public:
     
     pthread_mutex_t deleteMutex;
     
-    char getType();
+    char getType() const;
+    const char* getTypeName() const;
     void setType(char newType);
     uint16_t getAgentId();
     void setAgentId(uint16_t thisAgentId);

shared/src/AgentList.cpp

@@ -11,6 +11,7 @@
 #include <cstdlib>
 #include <cstdio>
 #include "AgentList.h"
+#include "AgentTypes.h"
 #include "PacketHeaders.h"
 #include "SharedUtil.h"
 
@@ -69,12 +70,12 @@ unsigned int AgentList::getSocketListenPort() {
 
 void AgentList::processAgentData(sockaddr *senderAddress, void *packetData, size_t dataBytes) {
     switch (((char *)packetData)[0]) {
-        case 'D': {
+        case PACKET_HEADER_DOMAIN: {
             // list of agents from domain server
             updateList((unsigned char *)packetData, dataBytes);
             break;
         }
-        case 'H': {
+        case PACKET_HEADER_HEAD_DATA: {
             // head data from another agent
             updateAgentWithData(senderAddress, packetData, dataBytes);
             break;
@@ -82,8 +83,7 @@ void AgentList::processAgentData(sockaddr *senderAddress, void *packetData, size
         case PACKET_HEADER_PING: {
             // ping from another agent
             //std::cout << "Got ping from " << inet_ntoa(((sockaddr_in *)senderAddress)->sin_addr) << "\n";
-            char reply[] = "R";
+            agentSocket.send(senderAddress, &PACKET_HEADER_PING_REPLY, 1);
-            agentSocket.send(senderAddress, reply, 1);  
             break;
         }
         case PACKET_HEADER_PING_REPLY: {
@@ -181,13 +181,13 @@ bool AgentList::addOrUpdateAgent(sockaddr *publicSocket, sockaddr *localSocket,
             newAgent.activatePublicSocket();
         }
         
-        if (newAgent.getType() == 'M' && audioMixerSocketUpdate != NULL) {
+        if (newAgent.getType() == AGENT_TYPE_MIXER && audioMixerSocketUpdate != NULL) {
             // this is an audio mixer
             // for now that means we need to tell the audio class
             // to use the local socket information the domain server gave us
             sockaddr_in *publicSocketIn = (sockaddr_in *)publicSocket;
             audioMixerSocketUpdate(publicSocketIn->sin_addr.s_addr, publicSocketIn->sin_port);
-        } else if (newAgent.getType() == 'V') {
+        } else if (newAgent.getType() == AGENT_TYPE_VOXEL) {
             newAgent.activatePublicSocket();
         }
         
@@ -200,7 +200,7 @@ bool AgentList::addOrUpdateAgent(sockaddr *publicSocket, sockaddr *localSocket,
         return true;
     } else {
         
-        if (agent->getType() == 'M' || agent->getType() == 'V') {
+        if (agent->getType() == AGENT_TYPE_MIXER || agent->getType() == AGENT_TYPE_VOXEL) {
             // until the Audio class also uses our agentList, we need to update
             // the lastRecvTimeUsecs for the audio mixer so it doesn't get killed and re-added continously
             agent->setLastRecvTimeUsecs(usecTimestampNow());
@@ -211,9 +211,10 @@ bool AgentList::addOrUpdateAgent(sockaddr *publicSocket, sockaddr *localSocket,
     }    
 }
 
-const char* AgentList::AGENTS_OF_TYPE_HEAD = "H";
+// XXXBHG - do we want to move these?
-const char* AgentList::AGENTS_OF_TYPE_VOXEL_AND_INTERFACE = "VI";
 const char* AgentList::AGENTS_OF_TYPE_VOXEL = "V";
+const char* AgentList::AGENTS_OF_TYPE_INTERFACE = "I";
+const char* AgentList::AGENTS_OF_TYPE_VOXEL_AND_INTERFACE = "VI";
 
 void AgentList::broadcastToAgents(char *broadcastData, size_t dataBytes,const char* agentTypes) {
     for(std::vector<Agent>::iterator agent = agents.begin(); agent != agents.end(); agent++) {
@@ -230,7 +231,7 @@ void AgentList::pingAgents() {
     *payload = PACKET_HEADER_PING;
     
     for(std::vector<Agent>::iterator agent = agents.begin(); agent != agents.end(); agent++) {
-        if (agent->getType() == 'I') {
+        if (agent->getType() == AGENT_TYPE_INTERFACE) {
             if (agent->getActiveSocket() != NULL) {
                 // we know which socket is good for this agent, send there
                 agentSocket.send(agent->getActiveSocket(), payload, 1);
@@ -269,7 +270,8 @@ void *removeSilentAgents(void *args) {
             
             pthread_mutex_t * agentDeleteMutex = &agent->deleteMutex;
             
-            if ((checkTimeUSecs - agent->getLastRecvTimeUsecs()) > AGENT_SILENCE_THRESHOLD_USECS && agent->getType() != 'V'
+            if ((checkTimeUSecs - agent->getLastRecvTimeUsecs()) > AGENT_SILENCE_THRESHOLD_USECS 
+            	&& agent->getType() != AGENT_TYPE_VOXEL
                 && pthread_mutex_trylock(agentDeleteMutex) == 0) {
                 
                 std::cout << "Killing agent " << &(*agent)  << "\n";
@@ -325,7 +327,7 @@ void *checkInWithDomainServer(void *args) {
             sockaddr_in tempAddress;
             memcpy(&tempAddress.sin_addr, pHostInfo->h_addr_list[0], pHostInfo->h_length);
             strcpy(DOMAIN_IP, inet_ntoa(tempAddress.sin_addr));
-            printf("Domain server %s: %s\n", DOMAIN_HOSTNAME, DOMAIN_IP);
+            printf("Domain server %s: \n", DOMAIN_HOSTNAME);
             
         } else {
             printf("Failed lookup domainserver\n");

shared/src/AgentList.h

@@ -67,10 +67,9 @@ public:
     void startDomainServerCheckInThread();
     void stopDomainServerCheckInThread();
     
-	static const char* AGENTS_OF_TYPE_HEAD;
-	static const char* AGENTS_OF_TYPE_VOXEL_AND_INTERFACE;
 	static const char* AGENTS_OF_TYPE_VOXEL;
-    
+	static const char* AGENTS_OF_TYPE_INTERFACE;
+	static const char* AGENTS_OF_TYPE_VOXEL_AND_INTERFACE;
 };
 
 int unpackAgentId(unsigned char *packedData, uint16_t *agentId);

shared/src/AgentTypes.h

@@ -0,0 +1,26 @@
+//
+//  AgentTypes.h
+//  hifi
+//
+//  Created by Brad Hefta-Gaub on 2013/04/09
+//
+//
+//  Single byte/character Agent Types used to identify various agents in the system.
+//  For example, an agent whose is 'V' is always a voxel server.
+//  
+
+#ifndef hifi_AgentTypes_h
+#define hifi_AgentTypes_h
+
+// NOTE: If you add a new AGENT_TYPE_XXX then you also should add a new AGENT_TYPE_NAME_XXX and a new "case" to the
+//       switch statement in Agent.cpp specifically Agent::getTypeName().
+//       If you don't then it will make things harder on your co-developers in debugging because the Agent
+//       class won't know the name and will report it as "Unknown".
+
+// Agent Type Codes
+const char AGENT_TYPE_DOMAIN = 'D';
+const char AGENT_TYPE_VOXEL = 'V';
+const char AGENT_TYPE_INTERFACE = 'I'; // could also be injector???
+const char AGENT_TYPE_MIXER = 'M';
+
+#endif

shared/src/PacketHeaders.h

@@ -5,6 +5,9 @@
 //  Created by Stephen Birarda on 4/8/13.
 //
 //
+//  The packet headers below refer to the first byte of a received UDP packet transmitted between
+//  any two Hifi components.  For example, a packet whose first byte is 'P' is always a ping packet.
+//  
 
 #ifndef hifi_PacketHeaders_h
 #define hifi_PacketHeaders_h
@@ -12,5 +15,12 @@
 const char PACKET_HEADER_DOMAIN = 'D';
 const char PACKET_HEADER_PING = 'P';
 const char PACKET_HEADER_PING_REPLY = 'R';
+const char PACKET_HEADER_HEAD_DATA = 'H';
+const char PACKET_HEADER_Z_COMMAND = 'Z';
+const char PACKET_HEADER_INJECT_AUDIO = 'I';
+const char PACKET_HEADER_SET_VOXEL = 'S';
+const char PACKET_HEADER_ERASE_VOXEL = 'E';
+const char PACKET_HEADER_VOXEL_DATA = 'V';
+const char PACKET_HEADER_TRANSMITTER_DATA = 't';
 
 #endif

shared/src/SharedUtil.cpp

@@ -131,7 +131,7 @@ bool cmdOptionExists(int argc, const char * argv[],const char* option) {
 //              corresponding to the closest voxel which encloses a cube with
 //              lower corners at x,y,z, having side of length S.  
 //              The input values x,y,z range 0.0 <= v < 1.0
-//              message should be either 'I' for insert or 'R' for remove
+//              message should be either 'S' for SET or 'E' for ERASE
 //
 // IMPORTANT:   The buffer is returned to you a buffer which you MUST delete when you are
 //              done with it.

shared/src/VoxelTree.cpp

@@ -10,6 +10,7 @@
 #include <cstdio>
 #include <cmath>
 #include "SharedUtil.h"
+#include "PacketHeaders.h"
 #include "CounterStats.h"
 #include "OctalCode.h"
 #include "VoxelTree.h"
@@ -309,7 +310,7 @@ unsigned char * VoxelTree::loadBitstreamBuffer(unsigned char *& bitstreamBuffer,
                 if (strcmp((char *)stopOctalCode, (char *)currentVoxelNode->octalCode) == 0) {
                     // this is is the root node for this packet
                     // add the leading V
-                    *(bitstreamBuffer++) = 'V';
+                    *(bitstreamBuffer++) = PACKET_HEADER_VOXEL_DATA;
                     
                     // add its octal code to the packet
                     int octalCodeBytes = bytesRequiredForCodeLength(*currentVoxelNode->octalCode);

tools/sendvoxels.php

@@ -68,15 +68,15 @@ if (empty($options['i']) && empty($options['zcommand'])) {
 	$filename = $options['i'];
 	$server = $options['s'];
 	$port = empty($options['p']) ? 40106 : $options['p'];
-	$command = empty($options['c']) ? 'I' : $options['c'];
+	$command = empty($options['c']) ? 'S' : $options['c'];
 	switch($command) {
-		case 'I':
+		case 'S':
-		case 'R':
+		case 'E':
 		case 'Z':
 			//$command is good
 		break;
 		default:
-			$command='I';// insert by default!
+			$command='S';// insert by default!
 	}
 
 	if ($options['testmode']) {

voxel/src/main.cpp

@@ -13,9 +13,11 @@
 #include <cstdio>
 #include <OctalCode.h>
 #include <AgentList.h>
+#include <AgentTypes.h>
 #include <VoxelTree.h>
 #include "VoxelAgentData.h"
 #include <SharedUtil.h>
+#include <PacketHeaders.h>
 
 #ifdef _WIN32
 #include "Syssocket.h"
@@ -44,7 +46,7 @@ const int PACKETS_PER_CLIENT_PER_INTERVAL = 2;
 
 const int MAX_VOXEL_TREE_DEPTH_LEVELS = 4;
 
-AgentList agentList('V', VOXEL_LISTEN_PORT);
+AgentList agentList(AGENT_TYPE_VOXEL, VOXEL_LISTEN_PORT);
 VoxelTree randomTree;
 
 bool wantColorRandomizer = false;
@@ -293,8 +295,8 @@ int main(int argc, const char * argv[])
     // loop to send to agents requesting data
     while (true) {
         if (agentList.getAgentSocket().receive(&agentPublicAddress, packetData, &receivedBytes)) {
-        	// XXXBHG: Hacked in support for 'I' insert command
+        	// XXXBHG: Hacked in support for 'S' SET command
-            if (packetData[0] == 'I') {
+            if (packetData[0] == PACKET_HEADER_SET_VOXEL) {
             	unsigned short int itemNumber = (*((unsigned short int*)&packetData[1]));
             	printf("got I - insert voxels - command from client receivedBytes=%ld itemNumber=%d\n",
             		receivedBytes,itemNumber);
@@ -335,18 +337,18 @@ int main(int argc, const char * argv[])
             	// after done inserting all these voxels, then reaverage colors
 				randomTree.reaverageVoxelColors(randomTree.rootNode);
             }
-            if (packetData[0] == 'R') {
+            if (packetData[0] == PACKET_HEADER_ERASE_VOXEL) {
 
             	// Send these bits off to the VoxelTree class to process them
-				printf("got Remove Voxels message, have voxel tree do the work... randomTree.processRemoveVoxelBitstream()\n");
+				printf("got Erase Voxels message, have voxel tree do the work... randomTree.processRemoveVoxelBitstream()\n");
             	randomTree.processRemoveVoxelBitstream((unsigned char*)packetData,receivedBytes);
 
             	// Now send this to the connected agents so they know to delete
 				printf("rebroadcasting delete voxel message to connected agents... agentList.broadcastToAgents()\n");
-				agentList.broadcastToAgents(packetData,receivedBytes,AgentList::AGENTS_OF_TYPE_HEAD);
+				agentList.broadcastToAgents(packetData,receivedBytes,AgentList::AGENTS_OF_TYPE_INTERFACE);
             	
             }
-            if (packetData[0] == 'Z') {
+            if (packetData[0] == PACKET_HEADER_Z_COMMAND) {
 
             	// the Z command is a special command that allows the sender to send the voxel server high level semantic
             	// requests, like erase all, or add sphere scene
@@ -371,12 +373,14 @@ int main(int argc, const char * argv[])
 
 				// Now send this to the connected agents so they can also process these messages
 				printf("rebroadcasting Z message to connected agents... agentList.broadcastToAgents()\n");
-				agentList.broadcastToAgents(packetData,receivedBytes,AgentList::AGENTS_OF_TYPE_HEAD);
+				agentList.broadcastToAgents(packetData,receivedBytes,AgentList::AGENTS_OF_TYPE_INTERFACE);
             }
-            if (packetData[0] == 'H') {
+            // If we got a PACKET_HEADER_HEAD_DATA, then we're talking to an AGENT_TYPE_INTERFACE, and we
+            // need to make sure we have it in our agentList.
+            if (packetData[0] == PACKET_HEADER_HEAD_DATA) {
                 if (agentList.addOrUpdateAgent(&agentPublicAddress,
                                                &agentPublicAddress,
-                                               packetData[0],
+                                               AGENT_TYPE_INTERFACE,
                                                agentList.getLastAgentId())) {
                     agentList.increaseAgentId();
                 }