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

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

interface/src/Camera.cpp

@@ -0,0 +1,37 @@
+//-----------------------------------------------------------
+//
+// 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()
+{
+	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 );
+}
+

interface/src/Camera.h

@@ -0,0 +1,49 @@
+//-----------------------------------------------------------
+//
+// 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>
+
+class Camera
+{
+public:
+	Camera();
+	
+	void update();
+	
+	void setYaw				( double		y ) { yaw				= y; }
+	void setPitch			( double		p ) { pitch				= p; }
+	void setRoll			( double		r ) { roll				= r; }
+	void setUp				( double		u ) { up				= u; }
+	void setDistance		( double		d ) { distance			= d; }
+	void setTargetPosition	( glm::dvec3	t ) { targetPosition	= t; };
+	void setPosition		( glm::dvec3	p ) { position			= p; };
+	void setOrientation		( Orientation	o ) { orientation.set(o); }
+
+	double		getYaw			() { return yaw;			}
+	double		getPitch		() { return pitch;			}
+	double		getRoll			() { return roll;			}
+	glm::dvec3	getPosition		() { return position;		}
+	Orientation	getOrientation	() { return orientation;	}
+
+private:
+
+	glm::dvec3	position;
+	glm::dvec3	targetPosition;
+	double		yaw;
+	double		pitch;
+	double		roll;
+	double		up;
+	double		distance;
+	Orientation	orientation;
+};
+
+#endif

interface/src/FieldOfView.cpp

@@ -15,95 +15,93 @@
 
 using namespace glm;
 
-FieldOfView::FieldOfView()
+FieldOfView::FieldOfView() :
-    :   mat_orientation(mat4(1.0f)),
+    _matOrientation(mat4(1.0f)),
-        vec_bounds_low(vec3(-1.0f,-1.0f,-1.0f)),
+    _vecBoundsLow(vec3(-1.0f,-1.0f,-1.0f)),
-        vec_bounds_high(vec3(1.0f,1.0f,1.0f)),
+    _vecBoundsHigh(vec3(1.0f,1.0f,1.0f)),
-        val_width(256.0f),
+    _valWidth(256.0f),
-        val_height(256.0f),
+    _valHeight(256.0f),
-        val_angle(0.61),
+    _valAngle(0.61),
-        val_zoom(1.0f), 
+    _valZoom(1.0f), 
-        enm_aspect_balancing(expose_less)
+    _enmAspectBalancing(expose_less) {
-{
 }
 
-mat4 FieldOfView::getViewerScreenXform() const
+mat4 FieldOfView::getViewerScreenXform() const {
-{
+
     mat4 projection;
     vec3 low, high;
     getFrustum(low, high);
 
     // perspective projection? determine correct near distance
-    if (val_angle != 0.0f)
+    if (_valAngle != 0.0f) {
-    {
+
         projection = translate(
                 frustum(low.x, high.x, low.y, high.y, low.z, high.z),
                 vec3(0.f, 0.f, -low.z) );
-    }
+    } else {
-    else
+
-    {
         projection = ortho(low.x, high.x, low.y, high.y, low.z, high.z);
     }
 
     return projection;
 }
 
-mat4 FieldOfView::getWorldViewerXform() const
+mat4 FieldOfView::getWorldViewerXform() const {
-{
+
-    return translate(affineInverse(mat_orientation),
+    return translate(affineInverse(_matOrientation),
-            vec3(0.0f, 0.0f, -vec_bounds_high.z) );
+            vec3(0.0f, 0.0f, -_vecBoundsHigh.z) );
 }
 
-mat4 FieldOfView::getWorldScreenXform() const
+mat4 FieldOfView::getWorldScreenXform() const {
-{
+
     return translate(
-            getViewerScreenXform() * affineInverse(mat_orientation),
+            getViewerScreenXform() * affineInverse(_matOrientation),
-            vec3(0.0f, 0.0f, -vec_bounds_high.z) );
+            vec3(0.0f, 0.0f, -_vecBoundsHigh.z) );
 }
 
-mat4 FieldOfView::getViewerWorldXform() const
+mat4 FieldOfView::getViewerWorldXform() const {
-{
+
-    vec3 n_translate = vec3(0.0f, 0.0f, vec_bounds_high.z);
+    vec3 n_translate = vec3(0.0f, 0.0f, _vecBoundsHigh.z);
 
     return translate(
             translate(mat4(1.0f), n_translate) 
-                * mat_orientation, -n_translate );
+                * _matOrientation, -n_translate );
 }
 
-float FieldOfView::getPixelSize() const
+float FieldOfView::getPixelSize() const {
-{
+
     vec3 low, high;
     getFrustum(low, high);
 
     return std::min(
-            abs(high.x - low.x) / val_width,
+            abs(high.x - low.x) / _valWidth,
-            abs(high.y - low.y) / val_height);
+            abs(high.y - low.y) / _valHeight);
 }
 
-void FieldOfView::getFrustum(vec3& low, vec3& high) const
+void FieldOfView::getFrustum(vec3& low, vec3& high) const {
-{
+
-    low = vec_bounds_low;
+    low = _vecBoundsLow;
-    high = vec_bounds_high;
+    high = _vecBoundsHigh;
 
     // start with uniform zoom
-    float inv_zoom = 1.0f / val_zoom;
+    float inv_zoom = 1.0f / _valZoom;
     float adj_x = inv_zoom, adj_y = inv_zoom;
 
     // balance aspect
-    if (enm_aspect_balancing != stretch)
+    if (_enmAspectBalancing != stretch) {
-    {
+
-        float f_aspect = (high.x - low.x) / (high.y - low.y);
+        float f_aspect = (high.x - low.x) / (high.y - low.y);
-        float vp_aspect = val_width / val_height;
+        float vp_aspect = _valWidth / _valHeight;
+
+        if ((_enmAspectBalancing == expose_more)
+                != (f_aspect > vp_aspect)) {
 
-        if ((enm_aspect_balancing == expose_more)
-                != (f_aspect > vp_aspect))
-        {
             // expose_more -> f_aspect <= vp_aspect <=> adj >= 1
             // expose_less -> f_aspect  > vp_aspect <=> adj <  1
             adj_x = vp_aspect / f_aspect;
-        }
+
-        else
+        } else {
-        {
+
             // expose_more -> f_aspect  > vp_aspect <=> adj >  1
             // expose_less -> f_aspect <= vp_aspect <=> adj <= 1
             adj_y = f_aspect / vp_aspect;
@@ -121,8 +119,8 @@ void FieldOfView::getFrustum(vec3& low, vec3& high) const
     // calc and apply near distance based on near diagonal and perspective
     float w = high.x - low.x, h = high.y - low.y;
     high.z -= low.z;
-    low.z = val_angle == 0.0f ? 0.0f :
+    low.z = _valAngle == 0.0f ? 0.0f :
-            sqrt(w*w+h*h) * 0.5f / tan(val_angle * 0.5f);
+            sqrt(w*w+h*h) * 0.5f / tan(_valAngle * 0.5f);
     high.z += low.z;
 }
 

interface/src/FieldOfView.h

@@ -11,39 +11,35 @@
 
 #include <glm/glm.hpp>
 
-/**
+// 
- * Viewing parameter encapsulation.
+// Viewing parameter encapsulation.
- */
+// 
-class FieldOfView
+class FieldOfView {
-{
+
-        glm::mat4   mat_orientation;
+        glm::mat4   _matOrientation;
-        glm::vec3   vec_bounds_low;
+        glm::vec3   _vecBoundsLow;
-        glm::vec3   vec_bounds_high;
+        glm::vec3   _vecBoundsHigh;
-        float       val_width;
+        float       _valWidth;
-        float       val_height;
+        float       _valHeight;
-        float       val_angle;
+        float       _valAngle;
-        float       val_zoom;
+        float       _valZoom;
-        int         enm_aspect_balancing;
+        int         _enmAspectBalancing;
     public:
 
         FieldOfView();
 
         // mutators
 
-        FieldOfView& setBounds(glm::vec3 const& low, glm::vec3 const& high)
+        FieldOfView& setBounds(glm::vec3 const& low, glm::vec3 const& high) {
-        { vec_bounds_low = low; vec_bounds_high = high; return *this; }
+                _vecBoundsLow = low; _vecBoundsHigh = high; return *this; }
 
-        FieldOfView& setOrientation(glm::mat4 const& matrix)
+        FieldOfView& setOrientation(glm::mat4 const& matrix) { _matOrientation = matrix; return *this; }
-        { mat_orientation = matrix; return *this; }
 
-        FieldOfView& setPerspective(float angle)
+        FieldOfView& setPerspective(float angle) { _valAngle = angle; return *this; }
-        { val_angle = angle; return *this; }
 
-        FieldOfView& setResolution(unsigned width, unsigned height)
+        FieldOfView& setResolution(unsigned width, unsigned height) { _valWidth = width; _valHeight = height; return *this; }
-        { val_width = width; val_height = height; return *this; }
 
-        FieldOfView& setZoom(float factor)
+        FieldOfView& setZoom(float factor) { _valZoom = factor; return *this; }
-        { val_zoom = factor; return *this; }
 
         enum aspect_balancing
         {
@@ -52,76 +48,75 @@ class FieldOfView
             stretch
         };
 
-        FieldOfView& setAspectBalancing(aspect_balancing v)
+        FieldOfView& setAspectBalancing(aspect_balancing v) { _enmAspectBalancing = v; return *this; }
-        { enm_aspect_balancing = v; return *this; }
 
         // dumb accessors
 
-        glm::mat4 const& getOrientation()    const   { return mat_orientation; }
+        glm::mat4 const& getOrientation()    const   { return _matOrientation; }
-        float            getWidthInPixels()  const   { return val_width; }
+        float            getWidthInPixels()  const   { return _valWidth; }
-        float            getHeightInPixels() const   { return val_height; }
+        float            getHeightInPixels() const   { return _valHeight; }
-        float            getPerspective()    const   { return val_angle; }
+        float            getPerspective()    const   { return _valAngle; }
 
         // matrices
 
-        /**
+        // 
-         * Returns a full transformation matrix to project world coordinates
+        // Returns a full transformation matrix to project world coordinates
-         * onto the screen.
+        // onto the screen.
-         */
+        // 
         glm::mat4 getWorldScreenXform() const;
 
-        /**
+        // 
-         * Transforms world coordinates to viewer-relative coordinates.
+        // Transforms world coordinates to viewer-relative coordinates.
-         *
+        //
-         * This matrix can be used as the modelview matrix in legacy GL code
+        // This matrix can be used as the modelview matrix in legacy GL code
-         * where the projection matrix is kept separately.
+        // where the projection matrix is kept separately.
-         */
+        // 
         glm::mat4 getWorldViewerXform() const;
 
-        /**
+        // 
-         * Returns the transformation to of viewer-relative coordinates back
+        // Returns the transformation to of viewer-relative coordinates back
-         * to world space. 
+        // to world space. 
-         *
+        //
-         * This matrix can be used to set up a coordinate system for avatar
+        // This matrix can be used to set up a coordinate system for avatar
-         * rendering.
+        // rendering.
-         */
+        // 
         glm::mat4 getViewerWorldXform() const;
 
-        /**
+        // 
-         * Returns the transformation of viewer-relative coordinates to the
+        // Returns the transformation of viewer-relative coordinates to the
-         * screen.
+        // screen.
-         *
+        //
-         * This matrix can be used as the projection matrix in legacy GL code.
+        // This matrix can be used as the projection matrix in legacy GL code.
-         */
+        // 
         glm::mat4 getViewerScreenXform() const;
 
 
         // other useful information
 
-        /**
+        // 
-         * Returns the size of a pixel in world space, that is the minimum
+        // Returns the size of a pixel in world space, that is the minimum
-         * in respect to x/y screen directions.
+        // in respect to x/y screen directions.
-         */
+        // 
         float getPixelSize() const;
 
-        /**
+        // 
-         * Returns the frustum as used for the projection matrices.
+        // Returns the frustum as used for the projection matrices.
-         * The result depdends on the bounds, eventually aspect correction
+        // The result depdends on the bounds, eventually aspect correction
-         * for the current resolution, the perspective angle (specified in
+        // for the current resolution, the perspective angle (specified in
-         * respect to diagonal) and zoom.
+        // respect to diagonal) and zoom.
-         */
+        // 
         void getFrustum(glm::vec3& low, glm::vec3& high) const;
 
-        /**
+        // 
-         * Returns the z-offset from the origin to where orientation ia
+        // Returns the z-offset from the origin to where orientation ia
-         * applied.
+        // applied.
-         */
+        // 
-        float getTransformOffset() const { return vec_bounds_high.z; }
+        float getTransformOffset() const { return _vecBoundsHigh.z; }
 
-        /**
+        // 
-         * Returns the aspect ratio.
+        // Returns the aspect ratio.
-         */
+        // 
-        float getAspectRatio() const { return val_height / val_width; }
+        float getAspectRatio() const { return _valHeight / _valWidth; }
 };
 
 #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.
 //
 
@@ -38,8 +39,13 @@ vector<unsigned char> iris_texture;
 unsigned int iris_texture_width = 512;
 unsigned int iris_texture_height = 256;
 
+
+
+//---------------------------------------------------
 Head::Head()
 {
+	initializeAvatar();
+
     position	= glm::vec3(0,0,0);
     velocity	= glm::vec3(0,0,0);
     thrust		= glm::vec3(0,0,0);
@@ -78,6 +84,8 @@ Head::Head()
     browAudioLift = 0.0;
     noise = 0;
 	
+	handOffset = glm::vec3( 0.0, 0.0, 0.0 );
+    
     sphere = NULL;
     
     hand = new Hand(glm::vec3(skinColor[0], skinColor[1], skinColor[2]));
@@ -91,7 +99,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 +156,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;
 }
 
+
+
+
+
+//---------------------------------------------------
 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,63 +235,200 @@ void Head::UpdatePos(float frametime, SerialInterface * serialInterface, int hea
     } 
 }
 
+
+
+
+//---------------------------------------------------
+void Head::setAvatarPosition( double x, double y, double z )
+{
+	avatar.position = glm::dvec3( 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 
+//---------------------------------------------------
 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;
+	*/
 	
-    if (driveKeys[FWD]) {
+	
-        thrust += THRUST_MAG*forward;
+	avatar.thrust = glm::dvec3( 0.0, 0.0, 0.0 );
-    }
+	    
-    if (driveKeys[BACK]) {
+    if (driveKeys[FWD]) 
-        thrust += -THRUST_MAG*forward;
+	{
-    }
+		//position.x += avatar.orientation.getFront().getX() * 0.01;
-    if (driveKeys[RIGHT]) {
+		//position.y += avatar.orientation.getFront().getY() * 0.01;
-        thrust.x += forward.z*-THRUST_LATERAL_MAG;
+		//position.z -= avatar.orientation.getFront().getZ() * 0.01;
-        thrust.z += forward.x*THRUST_LATERAL_MAG;
+
-    }
+		avatar.thrust.x += avatar.orientation.getFront().getX() * THRUST_MAG;
-    if (driveKeys[LEFT]) {
+		avatar.thrust.y += avatar.orientation.getFront().getY() * THRUST_MAG;
-        thrust.x += forward.z*THRUST_LATERAL_MAG;
+		avatar.thrust.z -= avatar.orientation.getFront().getZ() * THRUST_MAG;
-        thrust.z += forward.x*-THRUST_LATERAL_MAG;
+
-    }
+        //thrust += THRUST_MAG*forward;
-    if (driveKeys[UP]) {
-        thrust.y += -THRUST_VERTICAL_MAG;
-    }
-    if (driveKeys[DOWN]) {
-        thrust.y += THRUST_VERTICAL_MAG;
     }
 	
+    if (driveKeys[BACK]) 
+	{
+		//position.x -= avatar.orientation.getFront().getX() * 0.01;
+		//position.y -= avatar.orientation.getFront().getY() * 0.01;
+		//position.z += avatar.orientation.getFront().getZ() * 0.01;
+
+		avatar.thrust.x -= avatar.orientation.getFront().getX() * THRUST_MAG;
+		avatar.thrust.y -= avatar.orientation.getFront().getY() * THRUST_MAG;
+		avatar.thrust.z += avatar.orientation.getFront().getZ() * THRUST_MAG;
+
+        //thrust += -THRUST_MAG*forward;
+    }
+
+	
+    if (driveKeys[RIGHT]) 
+	{
+		//position.x += avatar.orientation.getRight().getX() * 0.01;
+		//position.y += avatar.orientation.getRight().getY() * 0.01;
+		//position.z -= avatar.orientation.getRight().getZ() * 0.01;
+		
+		avatar.thrust.x += avatar.orientation.getRight().getX() * THRUST_MAG;
+		avatar.thrust.y += avatar.orientation.getRight().getY() * THRUST_MAG;
+		avatar.thrust.z -= avatar.orientation.getRight().getZ() * THRUST_MAG;
+		
+        //thrust.x += forward.z*-THRUST_LATERAL_MAG;
+        //thrust.z += forward.x*THRUST_LATERAL_MAG;
+    }
+    if (driveKeys[LEFT]) 
+	{
+		//position.x -= avatar.orientation.getRight().getX() * 0.01;
+		//position.y -= avatar.orientation.getRight().getY() * 0.01;
+		//position.z += avatar.orientation.getRight().getZ() * 0.01;
+
+		avatar.thrust.x -= avatar.orientation.getRight().getX() * THRUST_MAG;
+		avatar.thrust.y -= avatar.orientation.getRight().getY() * THRUST_MAG;
+		avatar.thrust.z += avatar.orientation.getRight().getZ() * THRUST_MAG;
+
+        //thrust.x += forward.z*THRUST_LATERAL_MAG;
+        //thrust.z += forward.x*-THRUST_LATERAL_MAG;
+    }
+	
+
+    if (driveKeys[UP])
+	{
+		//position.x -= avatar.orientation.getUp().getX() * 0.01;
+		//position.y -= avatar.orientation.getUp().getY() * 0.01;
+		//position.z += avatar.orientation.getUp().getZ() * 0.01;
+
+		avatar.thrust.x -= avatar.orientation.getUp().getX() * THRUST_MAG;
+		avatar.thrust.y -= avatar.orientation.getUp().getY() * THRUST_MAG;
+		avatar.thrust.z += avatar.orientation.getUp().getZ() * THRUST_MAG;
+
+        //thrust.y += -THRUST_VERTICAL_MAG;
+    }
+    if (driveKeys[DOWN]) 
+	{
+		//position.x += avatar.orientation.getUp().getX() * 0.01;
+		//position.y += avatar.orientation.getUp().getY() * 0.01;
+		//position.z -= avatar.orientation.getUp().getZ() * 0.01;
+
+		avatar.thrust.x += avatar.orientation.getUp().getX() * THRUST_MAG;
+		avatar.thrust.y += avatar.orientation.getUp().getY() * THRUST_MAG;
+		avatar.thrust.z -= avatar.orientation.getUp().getZ() * THRUST_MAG;
+
+        //thrust.y += THRUST_VERTICAL_MAG;
+    }
+	
+    if (driveKeys[ROT_RIGHT]) 
+	{	
+		avatar.yawDelta -= 300.0 * deltaTime;
+    }
+    if (driveKeys[ROT_LEFT]) 
+	{	
+		avatar.yawDelta += 300.0 * 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 += avatar.thrust * (double)deltaTime;
+		
+	position += avatar.velocity * (double)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)
     {
@@ -276,6 +447,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 +465,8 @@ void Head::simulate(float deltaTime)
         }
     }
     
+	
+	
     const float DEGREES_BETWEEN_VIEWER_EYES = 3;
     const float DEGREES_TO_VIEWER_MOUTH = 7;
 
@@ -350,12 +525,26 @@ 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;
         
@@ -363,23 +552,37 @@ void Head::render(int faceToFace, int isMine)
     glEnable(GL_DEPTH_TEST);
     glPushMatrix();
     
-    glScalef(scale, scale, scale);
+	//glScalef(scale, scale, scale);
-    glTranslatef(leanSideways, 0.f, leanForward);
 
-    glRotatef(Yaw, 0, 1, 0);
+	glTranslatef
+	( 
+		avatar.bone[ AVATAR_BONE_HEAD ].worldPosition.x, 
+		avatar.bone[ AVATAR_BONE_HEAD ].worldPosition.y, 
+		avatar.bone[ AVATAR_BONE_HEAD ].worldPosition.z 
+	);
 
-    hand->render(1);
+	glScalef( 0.03, 0.03, 0.03 );
+
+
+	//glTranslatef(leanSideways, 0.f, leanForward);
+    
+	//glRotatef(Yaw, 0, 1, 0);
+
+    glRotatef( avatar.yaw, 0, 1, 0);
+    
+	//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);
         else glScalef(0.75, 1.0, 1.0);
+		
         glColor3fv(skinColor);
 
 
@@ -401,7 +604,6 @@ void Head::render(int faceToFace, int isMine)
             }
         glPopMatrix();
 
-
         // Eyebrows
         audioAttack = 0.9*audioAttack + 0.1*fabs(loudness - lastLoudness);
         lastLoudness = loudness;
@@ -510,8 +712,319 @@ void Head::render(int faceToFace, int isMine)
     glPopMatrix();
  }
  
+ 
+ 
+
+//---------------------------------------------------------
+void Head::setHandMovement( glm::dvec3 movement )
+{
+	handOffset = glm::dvec3( movement.x, -movement.y, movement.z );
+}
+
+
+
+//-----------------------------------------
+void Head::initializeAvatar()
+{
+	avatar.position = glm::dvec3( 0.0, 0.0, 0.0 );
+	avatar.velocity = glm::dvec3( 0.0, 0.0, 0.0 );
+	avatar.thrust	= glm::dvec3( 0.0, 0.0, 0.0 );
+	avatar.orientation.setToIdentity();
+	
+	avatar.yaw = 0.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].worldPosition = glm::dvec3( 0.0, 0.0, 0.0 );
+		//avatar.bone[b].offsetPosition = glm::dvec3( 0.0, 0.0, 0.0 );
+		avatar.bone[b].velocity = glm::dvec3( 0.0, 0.0, 0.0 );
+		avatar.bone[b].worldOrientation.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_NULL;
+	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_NULL;
+	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::dvec3(  0.0,   0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_MID_SPINE			].defaultPosePosition = glm::dvec3(  0.0,   0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_CHEST_SPINE		].defaultPosePosition = glm::dvec3(  0.0,   0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_NECK				].defaultPosePosition = glm::dvec3(  0.0,   0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_HEAD				].defaultPosePosition = glm::dvec3(  0.0,   0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_CHEST			].defaultPosePosition = glm::dvec3( -0.06,  0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_SHOULDER		].defaultPosePosition = glm::dvec3( -0.03,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_UPPER_ARM		].defaultPosePosition = glm::dvec3(  0.0,  -0.12, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_FOREARM		].defaultPosePosition = glm::dvec3(  0.0,  -0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_HAND			].defaultPosePosition = glm::dvec3(  0.0,  -0.05, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_CHEST		].defaultPosePosition = glm::dvec3(  0.06,  0.06, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER		].defaultPosePosition = glm::dvec3(  0.03,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM	].defaultPosePosition = glm::dvec3(  0.0,  -0.12, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_FOREARM		].defaultPosePosition = glm::dvec3(  0.0,  -0.1,  0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_HAND			].defaultPosePosition = glm::dvec3(  0.0,  -0.05, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_PELVIS		].defaultPosePosition = glm::dvec3( -0.05,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_THIGH			].defaultPosePosition = glm::dvec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_SHIN			].defaultPosePosition = glm::dvec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_LEFT_FOOT			].defaultPosePosition = glm::dvec3(  0.0,   0.0,  0.04 );
+	avatar.bone[ AVATAR_BONE_RIGHT_PELVIS		].defaultPosePosition = glm::dvec3(  0.05,  0.0,  0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_THIGH		].defaultPosePosition = glm::dvec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_SHIN			].defaultPosePosition = glm::dvec3(  0.0,  -0.15, 0.0  );
+	avatar.bone[ AVATAR_BONE_RIGHT_FOOT			].defaultPosePosition = glm::dvec3(  0.0,   0.0,  0.04 );
+
+	//----------------------------------------------------------------------------
+	// calculate bone length
+	//----------------------------------------------------------------------------
+	calculateBoneLengths();
+
+	//----------------------------------------------------------------------------
+	// generate world positions
+	//----------------------------------------------------------------------------
+	updateAvatarSkeleton();
+	
+	//----------------------------------------------------------------------------
+	// set offset positions = world positions
+	//----------------------------------------------------------------------------
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		//avatar.bone[b].offsetPosition = avatar.bone[b].worldPosition;
+	}	
+}
+
+
+
+
+//-----------------------------------------
+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 the body...
+	
+	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].worldOrientation.set( avatar.orientation );
+			avatar.bone[b].worldPosition = avatar.position;
+		}
+		else
+		{
+			avatar.bone[b].worldOrientation.set( avatar.bone[ avatar.bone[b].parent ].worldOrientation );
+			avatar.bone[b].worldPosition = avatar.bone[ avatar.bone[b].parent ].worldPosition;
+		}
+				
+		double xx	= glm::dot( avatar.bone[b].defaultPosePosition.x, avatar.bone[b].worldOrientation.getRight	().x )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.y, avatar.bone[b].worldOrientation.getRight	().y )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.z, avatar.bone[b].worldOrientation.getRight	().z );
+
+		double yy	= glm::dot( avatar.bone[b].defaultPosePosition.x, avatar.bone[b].worldOrientation.getUp		().x )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.y, avatar.bone[b].worldOrientation.getUp		().y )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.z, avatar.bone[b].worldOrientation.getUp		().z );
+
+		double zz	= glm::dot( avatar.bone[b].defaultPosePosition.x, avatar.bone[b].worldOrientation.getFront	().x )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.y, avatar.bone[b].worldOrientation.getFront	().y )
+					+ glm::dot( avatar.bone[b].defaultPosePosition.z, avatar.bone[b].worldOrientation.getFront	().z );
+				
+		glm::dvec3 rotatedBoneVector( xx, yy, zz );
+
+		//rotatedBonePosition.x = avatar.bone[b].defaultPosePosition.x;// * avatar.bone[b].worldOrientation.getFront().x;
+		//rotatedBonePosition.y = avatar.bone[b].defaultPosePosition.y;// * avatar.bone[b].worldOrientation.getFront().y;
+		//rotatedBonePosition.z = avatar.bone[b].defaultPosePosition.z;// * avatar.bone[b].worldOrientation.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].worldPosition += rotatedBoneVector;
+	}	
+	
+	//------------------------------------------------------------------------
+	// reset hand and elbow position according to hand movement
+	//------------------------------------------------------------------------
+	updateHandMovement();
+	
+	/*
+	glm::dvec3 v( avatar.bone[ AVATAR_BONE_RIGHT_HAND ].worldPosition );
+	v -= avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].worldPosition;
+	
+	double distance = glm::length(v);
+	if ( distance > avatar.maxArmLength )
+	{
+		avatar.bone[ AVATAR_BONE_RIGHT_UPPER_ARM ].worldPosition += v * 0.2;
+	}
+	*/
+	
+	
+	
+	/*
+	//------------------------------------------------------------------------
+	// update offset position
+	//------------------------------------------------------------------------
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		glm::dvec3 diff( avatar.bone[b].worldPosition );
+		diff -= avatar.bone[b].offsetPosition;
+		
+		avatar.bone[b].offsetPosition += diff * 0.1;
+	}	
+	*/
+	
+	
+}
+
+
+//-------------------------------
+double Head::getAvatarYaw()
+{
+	return avatar.yaw;
+}
+
+
+
+//-------------------------------
+void Head::updateHandMovement()
+{
+	//----------------------------------------------------------------
+	// adjust right hand and elbow according to hand offset
+	//----------------------------------------------------------------
+	avatar.bone[ AVATAR_BONE_RIGHT_HAND ].worldPosition += handOffset;	
+	glm::dvec3 armVector = avatar.bone[ AVATAR_BONE_RIGHT_HAND ].worldPosition;
+	armVector -= avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].worldPosition;
+
+	//-------------------------------------------------------------------------------
+	// test to see if right hand is being dragged beyond maximum arm length
+	//-------------------------------------------------------------------------------
+	double 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 ].worldPosition = avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].worldPosition;
+		glm::dvec3 armNormal = armVector / distance;
+		armVector = armNormal * avatar.maxArmLength;
+		distance = avatar.maxArmLength;
+		glm::dvec3 constrainedPosition = avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].worldPosition;
+		constrainedPosition += armVector;
+		avatar.bone[ AVATAR_BONE_RIGHT_HAND ].worldPosition = constrainedPosition;
+	}
+	
+	//-----------------------------------------------------------------------------
+	// set elbow position 
+	//-----------------------------------------------------------------------------
+	glm::dvec3 newElbowPosition = avatar.bone[ AVATAR_BONE_RIGHT_SHOULDER ].worldPosition;
+	newElbowPosition += armVector * ONE_HALF;
+	glm::dvec3 perpendicular = glm::dvec3( -armVector.y, armVector.x, armVector.z );
+	newElbowPosition += perpendicular * ( 1.0 - ( avatar.maxArmLength / distance ) ) * ONE_HALF;
+	avatar.bone[ AVATAR_BONE_RIGHT_FOREARM ].worldPosition = newElbowPosition;
+}
+
+
+
+
+//-----------------------------------------
+void Head::renderBody()
+{
+	glColor3fv(skinColor);
+
+	for (int b=0; b<NUM_AVATAR_BONES; b++)
+	{
+		glPushMatrix();
+			glTranslatef( avatar.bone[b].worldPosition.x, avatar.bone[b].worldPosition.y, avatar.bone[b].worldPosition.z );
+			glScalef( 0.02, 0.02, 0.02 );
+			glutSolidSphere( 1, 10, 5 );
+		glPopMatrix();
+	}
+}
+
+
+
+
+
 //  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 +1032,36 @@ 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 ].worldPosition.x, 
+			avatar.bone[ AVATAR_BONE_RIGHT_HAND ].worldPosition.y, 
+			avatar.bone[ AVATAR_BONE_RIGHT_HAND ].worldPosition.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 ].worldPosition ); 
+	
     // 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",
+    
+	sscanf
+	(
+		(char *)data, "H%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
 		&Pitch, &Yaw, &Roll,
 		&position.x, &position.y, &position.z,
 		&loudness, &averageLoudness,
-           &handPos.x, &handPos.y, &handPos.z);
+		&handPos.x, &handPos.y, &handPos.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,103 @@ 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;
+	glm::dvec3	worldPosition;
+	glm::dvec3	defaultPosePosition;
+	glm::dvec3	velocity;
+	double		yaw;
+	double		pitch;
+	double		roll;
+	Orientation	worldOrientation;
+	double		length;
+};
+
+struct Avatar
+{
+	glm::dvec3	position;
+	glm::dvec3	velocity;
+	glm::dvec3	thrust;
+	double		yaw;
+	double		pitch;
+	double		roll;
+	double		yawDelta;
+	double		maxArmLength;
+	Orientation	orientation;
+	AvatarBone	bone[ NUM_AVATAR_BONES ];
+};
+
 class Head : public AgentData {
     public:
         Head();
@@ -59,9 +157,19 @@ class Head : public AgentData {
         float getYaw() {return Yaw;}
         float getLastMeasuredYaw() {return YawRate;}
 		
+		double getAvatarYaw();
+        
         void render(int faceToFace, int isMine);
+		
+		void setAvatarPosition( double, double, double );
+		void renderBody();
+		void renderHead( int faceToFace, int isMine );
+
         void simulate(float);
 				
+		void setHandMovement( glm::dvec3 movement );
+		void updateHandMovement();
+        
         //  Send and receive network data
         int getBroadcastData(char * data);
         void parseData(void *data, int size);
@@ -125,12 +233,19 @@ class Head : public AgentData {
         glm::vec3 velocity;
         glm::vec3 thrust;
 		
+		glm::dvec3 handOffset;
+    
         int driveKeys[MAX_DRIVE_KEYS];
         
         int eyeContact;
         eyeContactTargets eyeContactTarget;
     
         GLUquadric *sphere;
+		Avatar avatar;
+		
+		void initializeAvatar();
+		void updateAvatarSkeleton();
+		void calculateBoneLengths();
     
         void readSensors();
         float renderYaw, renderPitch;       //   Pitch from view frustum when this is own head.

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

@@ -90,8 +90,8 @@ double diffclock(timeval *clock1,timeval *clock2)
 	return diffms;
 }
 
-void drawtext(int x, int y, float scale, float rotate, float thick, int mono, char *string, 
+void drawtext(int x, int y, float scale, float rotate, float thick, int mono,
-              float r, float g, float b)
+              char const* string, float r, float g, float b)
 {
     //
     //  Draws text on screen as stroked so it can be resized

interface/src/Util.h

@@ -17,14 +17,32 @@
 
 #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);
 
 float randFloat();
 void render_world_box();
 void render_vector(glm::vec3 * vec);
-void drawtext(int x, int y, float scale, float rotate, float thick, int mono, char *string, 
+void drawtext(int x, int y, float scale, float rotate, float thick, int mono, 
-              float r=1.0, float g=1.0, float b=1.0);
+              char const* string, float r=1.0, float g=1.0, float b=1.0);
 void drawvec3(int x, int y, float scale, float rotate, float thick, int mono, glm::vec3 vec, 
               float r=1.0, float g=1.0, float b=1.0);
 double diffclock(timeval *clock1,timeval *clock2);

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"
 
@@ -82,6 +83,30 @@ void VoxelSystem::createSphere(float r,float xc, float yc, float zc, float s, bo
     setupNewVoxelsForDrawing();
 }
 
+long int VoxelSystem::getVoxelsCreated() {
+    return tree->voxelsCreated;
+}
+
+long int VoxelSystem::getVoxelsCreatedRunningAverage() {
+    return tree->voxelsCreatedStats.getRunningAverage();
+}
+
+long int VoxelSystem::getVoxelsColored() {
+    return tree->voxelsColored;
+}
+
+long int VoxelSystem::getVoxelsColoredRunningAverage() {
+    return tree->voxelsColoredStats.getRunningAverage();
+}
+
+long int VoxelSystem::getVoxelsBytesRead() {
+    return tree->voxelsBytesRead;
+}
+
+long int VoxelSystem::getVoxelsBytesReadRunningAverage() {
+    return tree->voxelsBytesReadStats.getRunningAverage();
+}
+
 
 void VoxelSystem::parseData(void *data, int size) {
 
@@ -89,14 +114,37 @@ 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 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 
+            // messages differently
+            char* packetData =(char*)data;
+            char* command = &packetData[1]; // start of the command
+            int commandLength = strlen(command); // commands are null terminated strings
+            int totalLength = 1+commandLength+1;
+
+            printf("got Z message len(%d)= %s\n",size,command);
+
+            while (totalLength <= size) {
+                if (0==strcmp(command,(char*)"erase all")) {
+                    printf("got Z message == erase all\n");
+                    tree->eraseAllVoxels();
+                }
+                if (0==strcmp(command,(char*)"add scene")) {
+                    printf("got Z message == add scene - NOT SUPPORTED ON INTERFACE\n");
+                }
+                totalLength += commandLength+1;
+            }
+        break;
     }
     
     setupNewVoxelsForDrawing();
@@ -134,11 +182,34 @@ int VoxelSystem::treeToArrays(VoxelNode *currentNode, float nodePosition[3]) {
     float halfUnitForVoxel = powf(0.5, *currentNode->octalCode) * (0.5 * TREE_SCALE);
     glm::vec3 viewerPosition = viewerHead->getPos();
 
-    float distanceToVoxelCenter = sqrtf(powf(viewerPosition[0] - nodePosition[0] - halfUnitForVoxel, 2) +
+    // XXXBHG - Note: It appears as if the X and Z coordinates of Head or Agent are flip-flopped relative to the 
-                                        powf(viewerPosition[1] - nodePosition[1] - halfUnitForVoxel, 2) +
+    // coords of the voxel space. This flip flop causes LOD behavior to be extremely odd. This is my temporary hack 
-                                        powf(viewerPosition[2] - nodePosition[2] - halfUnitForVoxel, 2));
+    // to fix this behavior. To disable this swap, set swapXandZ to false.
+    bool swapXandZ=true;
+    float viewerX = swapXandZ ? viewerPosition[2] : viewerPosition[0];
+    float viewerZ = swapXandZ ? viewerPosition[0] : viewerPosition[2];
 
-    if (distanceToVoxelCenter < boundaryDistanceForRenderLevel(*currentNode->octalCode + 1)) {
+    // debugging code.
+    //printf("treeToArrays() halfUnitForVoxel=%f\n",halfUnitForVoxel);
+    //printf("treeToArrays() viewerPosition {x,y,z or [0],[1],[2]} ={%f,%f,%f}\n",
+    //      viewerPosition[0],viewerPosition[1],viewerPosition[2]);
+    //printf("treeToArrays() nodePosition   {x,y,z or [0],[1],[2]} = {%f,%f,%f}\n",
+    //      nodePosition[0],nodePosition[1],nodePosition[2]);
+    //float* vertices = firstVertexForCode(currentNode->octalCode);
+    //printf("treeToArrays() firstVerticesForCode(currentNode->octalCode)={x,y,z or [0],[1],[2]} = {%f,%f,%f}\n",
+    //      vertices[0],vertices[1],vertices[2]);
+    //delete []vertices;
+    
+    float distanceToVoxelCenter = sqrtf(powf(viewerX - nodePosition[0] - halfUnitForVoxel, 2) +
+                                        powf(viewerPosition[1] - nodePosition[1] - halfUnitForVoxel, 2) +
+                                        powf(viewerZ - nodePosition[2] - halfUnitForVoxel, 2));
+
+    int boundaryPosition = boundaryDistanceForRenderLevel(*currentNode->octalCode + 1);
+    //printf("treeToArrays() distanceToVoxelCenter=%f boundaryPosition=%d\n",distanceToVoxelCenter,boundaryPosition);
+
+    bool alwaysDraw = false; // XXXBHG - temporary debug code. Flip this to true to disable LOD blurring
+
+    if (alwaysDraw || distanceToVoxelCenter < boundaryPosition) {
         for (int i = 0; i < 8; i++) {
             // check if there is a child here
             if (currentNode->children[i] != NULL) {
@@ -155,8 +226,6 @@ int VoxelSystem::treeToArrays(VoxelNode *currentNode, float nodePosition[3]) {
                         childNodePosition[j] -= (powf(0.5, *currentNode->children[i]->octalCode) * TREE_SCALE);
                     }
                 }
-                
-                
                 voxelsAdded += treeToArrays(currentNode->children[i], childNodePosition);
             }
         }

interface/src/VoxelSystem.h

@@ -37,6 +37,14 @@ public:
     void setViewerHead(Head *newViewerHead);
     void loadVoxelsFile(const char* fileName,bool wantColorRandomizer);
 	void createSphere(float r,float xc, float yc, float zc, float s, bool solid, bool wantColorRandomizer);
+
+    long int getVoxelsCreated();
+    long int getVoxelsColored();
+    long int getVoxelsBytesRead();
+    long int getVoxelsCreatedRunningAverage();
+    long int getVoxelsColoredRunningAverage();
+    long int getVoxelsBytesReadRunningAverage();
+
 private:
     int voxelsRendered;
     Head *viewerHead;

interface/src/main.cpp

@@ -50,6 +50,7 @@
 
 #include "Head.h"
 #include "Hand.h"
+#include "Camera.h"
 #include "Particle.h"
 #include "Texture.h"
 #include "Cloud.h"
@@ -62,10 +63,11 @@
 #include "SerialInterface.h"
 #include <PerfStat.h>
 #include <SharedUtil.h>
+#include <PacketHeaders.h>
 
 using namespace std;
 
-int audio_on = 0;                   //  Whether to turn on the audio support
+int audio_on = 1;                   //  Whether to turn on the audio support
 int simulate_on = 1; 
 
 AgentList agentList('I');
@@ -95,9 +97,11 @@ Oscilloscope audioScope(256,200,true);
 
 #define HAND_RADIUS 0.25            //  Radius of in-world 'hand' of you
 Head myHead;                        //  The rendered head of oneself
+Camera myCamera;					//  My view onto the world (sometimes on myself :)
 
 char starFile[] = "https://s3-us-west-1.amazonaws.com/highfidelity/stars.txt";
 FieldOfView fov;
+
 Stars stars;
 #ifdef STARFIELD_KEYS
 int starsTiles = 20;
@@ -142,7 +146,11 @@ float renderPitchRate = 0.f;
 glm::vec3 start_location(6.1f, 0, 1.4f);
 
 int stats_on = 0;					//  Whether to show onscreen text overlay with stats
-
+bool starsOn = false;				//  Whether to display the stars
+bool paintOn = false;				//  Whether to paint voxels as you fly around
+VoxelDetail paintingVoxel;			//	The voxel we're painting if we're painting
+unsigned char dominantColor = 0;	//	The dominant color of the voxel we're painting
+bool perfStatsOn = false;			//  Do we want to display perfStats?
 int noise_on = 0;					//  Whether to add random noise 
 float noise = 1.0;                  //  Overall magnitude scaling for random noise levels 
 
@@ -188,7 +196,6 @@ char texture_filename[] = "images/int-texture256-v4.png";
 unsigned int texture_width = 256;
 unsigned int texture_height = 256;
 
-
 float particle_attenuation_quadratic[] =  { 0.0f, 0.0f, 2.0f }; // larger Z = smaller particles
 float pointer_attenuation_quadratic[] =  { 1.0f, 0.0f, 0.0f }; // for 2D view
 
@@ -245,16 +252,24 @@ void Timer(int extra)
     }
 }
 
+
+
+
 void display_stats(void)
 {
 	//  bitmap chars are about 10 pels high 
     char legend[] = "/ - toggle this display, Q - exit, H - show head, M - show hand, T - test audio";
     drawtext(10, 15, 0.10f, 0, 1.0, 0, legend);
 
+    char legend2[] = "* - toggle stars, & - toggle paint mode, '-' - send erase all, '%' - send add scene";
+    drawtext(10, 32, 0.10f, 0, 1.0, 0, legend2);
+
+	glm::vec3 headPos = myHead.getPos();
+    
     char stats[200];
-    sprintf(stats, "FPS = %3.0f  Pkts/s = %d  Bytes/s = %d ", 
+    sprintf(stats, "FPS = %3.0f  Pkts/s = %d  Bytes/s = %d Head(x,y,z)=( %f , %f , %f )", 
-            FPS, packets_per_second,  bytes_per_second);
+            FPS, packets_per_second,  bytes_per_second, headPos.x,headPos.y,headPos.z);
-    drawtext(10, 30, 0.10f, 0, 1.0, 0, stats); 
+    drawtext(10, 49, 0.10f, 0, 1.0, 0, stats); 
     if (serialPort.active) {
         sprintf(stats, "ADC samples = %d, LED = %d", 
                 serialPort.getNumSamples(), serialPort.getLED());
@@ -273,6 +288,32 @@ void display_stats(void)
     voxelStats << "Voxels Rendered: " << voxels.getVoxelsRendered();
     drawtext(10,70,0.10f, 0, 1.0, 0, (char *)voxelStats.str().c_str());
 
+	voxelStats.str("");
+	voxelStats << "Voxels Created: " << voxels.getVoxelsCreated() << " (" << voxels.getVoxelsCreatedRunningAverage() 
+		<< "/sec in last "<< COUNTETSTATS_TIME_FRAME << " seconds) ";
+    drawtext(10,250,0.10f, 0, 1.0, 0, (char *)voxelStats.str().c_str());
+
+	voxelStats.str("");
+	voxelStats << "Voxels Colored: " << voxels.getVoxelsColored() << " (" << voxels.getVoxelsColoredRunningAverage() 
+		<< "/sec in last "<< COUNTETSTATS_TIME_FRAME << " seconds) ";
+    drawtext(10,270,0.10f, 0, 1.0, 0, (char *)voxelStats.str().c_str());
+	
+	voxelStats.str("");
+	voxelStats << "Voxels Bytes Read: " << voxels.getVoxelsBytesRead()  
+		<< " (" << voxels.getVoxelsBytesReadRunningAverage() << "/sec in last "<< COUNTETSTATS_TIME_FRAME << " seconds) ";
+    drawtext(10,290,0.10f, 0, 1.0, 0, (char *)voxelStats.str().c_str());
+
+	voxelStats.str("");
+	long int voxelsBytesPerColored = voxels.getVoxelsColored() ? voxels.getVoxelsBytesRead()/voxels.getVoxelsColored() : 0;
+	long int voxelsBytesPerColoredAvg = voxels.getVoxelsColoredRunningAverage() ? 
+		voxels.getVoxelsBytesReadRunningAverage()/voxels.getVoxelsColoredRunningAverage() : 0;
+
+	voxelStats << "Voxels Bytes per Colored: " << voxelsBytesPerColored  
+		<< " (" << voxelsBytesPerColoredAvg << "/sec in last "<< COUNTETSTATS_TIME_FRAME << " seconds) ";
+    drawtext(10,310,0.10f, 0, 1.0, 0, (char *)voxelStats.str().c_str());
+	
+
+	if (::perfStatsOn) {
 		// Get the PerfStats group details. We need to allocate and array of char* long enough to hold 1+groups
 		char** perfStatLinesArray = new char*[PerfStat::getGroupCount()+1];
 		int lines = PerfStat::DumpStats(perfStatLinesArray);
@@ -284,6 +325,7 @@ void display_stats(void)
 			atZ+=20; // height of a line
 		}
 		delete []perfStatLinesArray; // we're responsible for cleanup
+	}
 	        
     /*
     std::stringstream angles;
@@ -317,6 +359,9 @@ void initDisplay(void)
     if (fullscreen) glutFullScreen();
 }
 
+
+
+
 void init(void)
 {
     voxels.init();
@@ -337,7 +382,9 @@ void init(void)
     if (noise_on) {   
         myHead.setNoise(noise);
     }
-    myHead.setPos(start_location);
+    myHead.setPos(start_location );
+	
+	myCamera.setPosition( glm::dvec3( start_location ) );
 	
 #ifdef MARKER_CAPTURE
     if(marker_capture_enabled){
@@ -351,7 +398,6 @@ void init(void)
     }
 #endif
     
-    
     gettimeofday(&timer_start, NULL);
     gettimeofday(&last_frame, NULL);
 }
@@ -418,6 +464,11 @@ void simulateHead(float frametime)
     
     myHead.UpdatePos(frametime, &serialPort, head_mirror, &gravity);
 	
+	//-------------------------------------------------------------------------------------
+	// set the position of the avatar
+	//-------------------------------------------------------------------------------------
+	myHead.setAvatarPosition( -myHead.getPos().x, -myHead.getPos().y, -myHead.getPos().z );
+	
     //  Update head_mouse model 
     const float MIN_MOUSE_RATE = 30.0;
     const float MOUSE_SENSITIVITY = 0.1f;
@@ -485,17 +536,42 @@ void simulateHead(float frametime)
     char broadcast_string[MAX_BROADCAST_STRING];
     int broadcast_bytes = myHead.getBroadcastData(broadcast_string);
     agentList.broadcastToAgents(broadcast_string, broadcast_bytes,AgentList::AGENTS_OF_TYPE_VOXEL_AND_INTERFACE);
+
+    // If I'm in paint mode, send a voxel out to VOXEL server agents.
+    if (::paintOn) {
+    
+    	glm::vec3 headPos = myHead.getPos();
+
+		// For some reason, we don't want to flip X and Z here.
+		::paintingVoxel.x = headPos.x/-10.0;  
+		::paintingVoxel.y = headPos.y/-10.0;  
+		::paintingVoxel.z = headPos.z/-10.0;
+    	
+    	unsigned char* bufferOut;
+    	int sizeOut;
+    	
+		if (::paintingVoxel.x >= 0.0 && ::paintingVoxel.x <= 1.0 &&
+			::paintingVoxel.y >= 0.0 && ::paintingVoxel.y <= 1.0 &&
+			::paintingVoxel.z >= 0.0 && ::paintingVoxel.z <= 1.0) {
+
+			if (createVoxelEditMessage(PACKET_HEADER_SET_VOXEL,0,1,&::paintingVoxel,bufferOut,sizeOut)){
+				agentList.broadcastToAgents((char*)bufferOut, sizeOut,AgentList::AGENTS_OF_TYPE_VOXEL);
+				delete bufferOut;
+			}
+		}
+    }
 }
 
 int render_test_spot = WIDTH/2;
 int render_test_direction = 1; 
 
+
+
+
 void display(void)
 {
 	PerfStat("display");
 
-    glEnable (GL_DEPTH_TEST);
-    glEnable(GL_LIGHTING);
     glEnable(GL_LINE_SMOOTH);
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
     glMatrixMode(GL_MODELVIEW);
@@ -519,19 +595,53 @@ void display(void)
         glMaterialfv(GL_FRONT, GL_SPECULAR, specular_color);
         glMateriali(GL_FRONT, GL_SHININESS, 96);
 
-        //  Rotate, translate to camera location
+		//-------------------------------------------------------------------------------------
-        fov.setOrientation(
+		// set the camera to third-person view
-            glm::rotate(glm::rotate(glm::translate(glm::mat4(1.0f), -myHead.getPos()), 
+		//-------------------------------------------------------------------------------------		
-                -myHead.getRenderYaw(), glm::vec3(0.0f,1.0f,0.0f)),
+		myCamera.setTargetPosition( (glm::dvec3)myHead.getPos() );	
-                -myHead.getRenderPitch(), glm::vec3(1.0f,0.0f,0.0f)) );
+		myCamera.setPitch	( 0.0 );
+		myCamera.setRoll	( 0.0 );
 
-        glLoadMatrixf( glm::value_ptr(fov.getWorldViewerXform()) );
+		if ( display_head )
-        glRotatef(myHead.getRenderPitch(), 1, 0, 0);
+		//-------------------------------------------------------------------------------------
-        glRotatef(myHead.getRenderYaw(), 0, 1, 0);
+		// set the camera to looking at my face
+		//-------------------------------------------------------------------------------------		
+		{
+			myCamera.setYaw		( - myHead.getAvatarYaw() );
+			myCamera.setUp		( 0.4  );
+			myCamera.setDistance( 0.08 );	
+			myCamera.update();
+		}
+		else
+		//-------------------------------------------------------------------------------------
+		// set the camera to third-person view
+		//-------------------------------------------------------------------------------------		
+		{
+			myCamera.setYaw		( 180.0 - myHead.getAvatarYaw() );
+			myCamera.setUp		( 0.15 );
+			myCamera.setDistance( 0.08 );	
+			myCamera.update();
+		}
 		
-        glDisable(GL_LIGHTING);
+		//-------------------------------------------------------------------------------------
-        glDisable(GL_DEPTH_TEST);
+		// transform to camera view
+		//-------------------------------------------------------------------------------------
+        glRotatef	( myCamera.getPitch(),	1, 0, 0 );
+        glRotatef	( myCamera.getYaw(),	0, 1, 0 );
+        glRotatef	( myCamera.getRoll(),	0, 0, 1 );
+		
+		//printf( "myCamera position = %f, %f, %f\n", myCamera.getPosition().x, myCamera.getPosition().y, myCamera.getPosition().z );		
+		
+        glTranslatef( myCamera.getPosition().x, myCamera.getPosition().y, myCamera.getPosition().z );
+        
+		// fixed view
+		//glTranslatef( 6.18, -0.15, 1.4 );
+
+        if (::starsOn) {
+            // should be the first rendering pass - w/o depth buffer / lighting
         	stars.render(fov);
+        }
+
         glEnable(GL_LIGHTING);
         glEnable(GL_DEPTH_TEST);
         
@@ -566,12 +676,20 @@ void display(void)
         //  Render the world box
         if (!display_head && stats_on) render_world_box();
     
+	
+		//---------------------------------
         //  Render my own head
+		//---------------------------------
+		myHead.render( true, 1 );	
+	
+		/*
         glPushMatrix();
         glLoadIdentity();
         glTranslatef(0.f, 0.f, -7.f);
         myHead.render(display_head, 1);
         glPopMatrix();
+		*/
+		
     }
     
     glPopMatrix();
@@ -635,13 +753,26 @@ void display(void)
     if (display_levels) serialPort.renderLevels(WIDTH,HEIGHT);
     
     //  Display miscellaneous text stats onscreen
-    if (stats_on) display_stats();
+    if (stats_on) {
+        glLineWidth(1.0f);
+        glPointSize(1.0f);
+        display_stats();
+    }
 
     //  Draw number of nearby people always
     char agents[100];
     sprintf(agents, "Agents nearby: %ld\n", agentList.getAgents().size());
     drawtext(WIDTH-200,20, 0.10, 0, 1.0, 0, agents, 1, 1, 0);
     
+    if (::paintOn) {
+    
+		char paintMessage[100];
+		sprintf(paintMessage,"Painting (%.3f,%.3f,%.3f/%.3f/%d,%d,%d)",
+			::paintingVoxel.x,::paintingVoxel.y,::paintingVoxel.z,::paintingVoxel.s,
+			(unsigned int)::paintingVoxel.red,(unsigned int)::paintingVoxel.green,(unsigned int)::paintingVoxel.blue);
+		drawtext(WIDTH-350,40, 0.10, 0, 1.0, 0, paintMessage, 1, 1, 0);
+    }
+    
     glPopMatrix();
     
 
@@ -649,6 +780,10 @@ void display(void)
     framecount++;
 }
 
+
+
+
+
 void testPointToVoxel()
 {
 	float y=0;
@@ -669,6 +804,41 @@ void testPointToVoxel()
 	}
 }
 
+void sendVoxelServerEraseAll() {
+	char message[100];
+    sprintf(message,"%c%s",'Z',"erase all");
+	int messageSize = strlen(message)+1;
+	::agentList.broadcastToAgents(message, messageSize,AgentList::AGENTS_OF_TYPE_VOXEL);
+}
+
+void sendVoxelServerAddScene() {
+	char message[100];
+    sprintf(message,"%c%s",'Z',"add scene");
+	int messageSize = strlen(message)+1;
+	::agentList.broadcastToAgents(message, messageSize,AgentList::AGENTS_OF_TYPE_VOXEL);
+}
+
+void shiftPaintingColor()
+{
+    // About the color of the paintbrush... first determine the dominant color
+    ::dominantColor = (::dominantColor+1)%3; // 0=red,1=green,2=blue
+	::paintingVoxel.red   = (::dominantColor==0)?randIntInRange(200,255):randIntInRange(40,100);
+	::paintingVoxel.green = (::dominantColor==1)?randIntInRange(200,255):randIntInRange(40,100);
+	::paintingVoxel.blue  = (::dominantColor==2)?randIntInRange(200,255):randIntInRange(40,100);
+}
+
+void setupPaintingVoxel()
+{
+	glm::vec3 headPos = myHead.getPos();
+
+	::paintingVoxel.x = headPos.z/-10.0;	// voxel space x is negative z head space
+	::paintingVoxel.y = headPos.y/-10.0;  // voxel space y is negative y head space
+	::paintingVoxel.z = headPos.x/-10.0;  // voxel space z is negative x head space
+	::paintingVoxel.s = 1.0/256;
+	
+	shiftPaintingColor();
+}
+
 void addRandomSphere(bool wantColorRandomizer)
 {
 	float r = randFloatInRange(0.05,0.1);
@@ -753,8 +923,15 @@ void key(unsigned char k, int x, int y)
     
 	//  Process keypresses 
  	if (k == 'q')  ::terminate();
-    
     if (k == '/')  stats_on = !stats_on;		// toggle stats
+    if (k == '*')  ::starsOn = !::starsOn;		// toggle stars
+    if (k == '&') {
+    	::paintOn = !::paintOn;		// toggle paint
+    	::setupPaintingVoxel();		// also randomizes colors
+    }
+    if (k == '^')  ::shiftPaintingColor();		// shifts randomize color between R,G,B dominant
+    if (k == '-')  ::sendVoxelServerEraseAll();	// sends erase all command to voxel server
+    if (k == '%')  ::sendVoxelServerAddScene();	// sends add scene command to voxel server
 	if (k == 'n') 
     {
         noise_on = !noise_on;                   // Toggle noise 
@@ -810,11 +987,7 @@ void key(unsigned char k, int x, int y)
     }
 
 	// press the . key to get a new random sphere of voxels added 
-    if (k == '.')
+    if (k == '.') addRandomSphere(wantColorRandomizer);
-    {
-        addRandomSphere(wantColorRandomizer);
-        //testPointToVoxel();
-    }
 }
 
 //
@@ -831,10 +1004,12 @@ void *networkReceive(void *args)
             packetcount++;
             bytescount += bytesReceived;
             
-            if (incomingPacket[0] == 't') {
+            if (incomingPacket[0] == PACKET_HEADER_TRANSMITTER_DATA) {
                 //  Pass everything but transmitter data to the agent list
                  myHead.hand->processTransmitterData(incomingPacket, bytesReceived);            
-            } else if (incomingPacket[0] == 'V' || incomingPacket[0] == 'R') {
+            } else if (incomingPacket[0] == PACKET_HEADER_VOXEL_DATA || 
+					incomingPacket[0] == PACKET_HEADER_Z_COMMAND || 
+					incomingPacket[0] == PACKET_HEADER_ERASE_VOXEL) {
                 voxels.parseData(incomingPacket, bytesReceived);
             } else {
                agentList.processAgentData(&senderAddress, incomingPacket, bytesReceived);
@@ -855,6 +1030,23 @@ void idle(void)
     if (diffclock(&last_frame, &check) > RENDER_FRAME_MSECS)
     {
         steps_per_frame++;
+		
+		//----------------------------------------------------------------
+		// If mouse is being dragged, update hand movement in the avatar
+		//----------------------------------------------------------------
+		if ( mouse_pressed == 1 )
+		{
+			double xOffset = ( mouse_x - mouse_start_x ) / (double)WIDTH;
+			double yOffset = ( mouse_y - mouse_start_y ) / (double)HEIGHT;
+			
+			double leftRight	= xOffset;
+			double downUp		= yOffset;
+			double backFront	= 0.0;
+			
+			glm::dvec3 handMovement( leftRight, downUp, backFront );
+			myHead.setHandMovement( handMovement );		
+		}		
+		
         //  Simulation
         simulateHead(1.f/FPS);
         simulateHand(1.f/FPS);
@@ -879,6 +1071,8 @@ void idle(void)
     }
 }
 
+
+
 void reshape(int width, int height)
 {
     WIDTH = width;
@@ -896,6 +1090,8 @@ void reshape(int width, int height)
     glViewport(0, 0, width, height);
 }
 
+
+
 void mouseFunc( int button, int state, int x, int y ) 
 {
     if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN )

interface/src/starfield/renderer/Renderer.h

@@ -96,18 +96,22 @@ namespace starfield {
             Tiling tiling(k);
             size_t nTiles = tiling.getTileCount();
 
+            // REVISIT: could coalesce allocation for faster rebuild
+            // REVISIT: batch arrays are probably oversized, but - hey - they
+            // are not very large (unless for insane tiling) and we're better
+            // off safe than sorry
             _arrData = new GpuVertex[n];
             _arrTile = new Tile[nTiles + 1];
-            _arrBatchOffs = new GLint[nTiles]; 
+            _arrBatchOffs = new GLint[nTiles * 2]; 
-            _arrBatchCount = new GLsizei[nTiles];
+            _arrBatchCount = new GLsizei[nTiles * 2];
 
             prepareVertexData(src, n, tiling, b, bMin);
 
             this->glUpload(n);
         }
 
-        ~Renderer()
+        ~Renderer() {
-        {
+
             delete[] _arrData;
             delete[] _arrTile;
             delete[] _arrBatchCount;
@@ -119,8 +123,7 @@ namespace starfield {
         void render(float perspective,
                     float aspect,
                     mat4 const& orientation,
-                    BrightnessLevel minBright)
+                    BrightnessLevel minBright) {
-        {
 
 // fprintf(stderr, "
 //      Stars.cpp: rendering at minimal brightness %d\n", minBright);
@@ -291,21 +294,31 @@ namespace starfield {
             bool select(Tile* t) {
 
                 if (t < _arrTile || t >= _itrTilesEnd ||
-                        !! (t->flags & Tile::visited)) {
+                        !! (t->flags & Tile::checked)) {
 
+                    // out of bounds or been here already
                     return false; 
                 }
-                if (! (t->flags & Tile::checked)) {
 
-                    if (_refRenderer.visitTile(t))
+                // will check now and never again
+                t->flags |= Tile::checked;
+                if (_refRenderer.visitTile(t)) {
+
+                    // good one -> remember (for batching) and propagate
                     t->flags |= Tile::render;
+                    return true;
                 }
-                return !! (t->flags & Tile::render);
+                return false;
             }
 
-            void process(Tile* t) { 
+            bool process(Tile* t) { 
+
+                if (! (t->flags & Tile::visited)) {
 
                     t->flags |= Tile::visited;
+                    return true;
+                }
+                return false;
             }
 
             void right(Tile*& cursor) const   { cursor += 1; }
@@ -479,6 +492,9 @@ namespace starfield {
 //     fprintf(stderr, "Stars.cpp: Batch #%d - %d stars @ %d\n", i, 
 //             _arrBatchOffs[i], _arrBatchCount[i]);
 
+            glDisable(GL_DEPTH_TEST);
+            glDisable(GL_LIGHTING);
+
             // setup modelview matrix (identity)
             glMatrixMode(GL_MODELVIEW);
             glPushMatrix();
@@ -507,6 +523,7 @@ namespace starfield {
             glBindVertexArray(0); 
             glUseProgram(0);
             glDisable(GL_VERTEX_PROGRAM_POINT_SIZE);
+            glDisable(GL_POINT_SMOOTH);
             glPopMatrix();
             glMatrixMode(GL_MODELVIEW);
             glPopMatrix();

mixer/src/main.cpp

@@ -20,6 +20,7 @@
 #include <SharedUtil.h>
 #include <StdDev.h>
 #include "AudioRingBuffer.h"
+#include "PacketHeaders.h"
 
 #ifdef _WIN32
 #include "Syssocket.h"
@@ -275,7 +276,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) {

shared/src/AgentList.cpp

@@ -11,6 +11,7 @@
 #include <cstdlib>
 #include <cstdio>
 #include "AgentList.h"
+#include "PacketHeaders.h"
 #include "SharedUtil.h"
 
 #ifdef _WIN32
@@ -68,24 +69,24 @@ 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;
         }
-        case 'P': {
+        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, reply, 1);  
             break;
         }
-        case 'R': {
+        case PACKET_HEADER_PING_REPLY: {
             // ping reply from another agent
             //std::cout << "Got ping reply from " << inet_ntoa(((sockaddr_in *)senderAddress)->sin_addr) << "\n";
             handlePingReply(senderAddress);
@@ -212,6 +213,7 @@ bool AgentList::addOrUpdateAgent(sockaddr *publicSocket, sockaddr *localSocket,
 
 const char* AgentList::AGENTS_OF_TYPE_HEAD = "H";
 const char* AgentList::AGENTS_OF_TYPE_VOXEL_AND_INTERFACE = "VI";
+const char* AgentList::AGENTS_OF_TYPE_VOXEL = "V";
 
 void AgentList::broadcastToAgents(char *broadcastData, size_t dataBytes,const char* agentTypes) {
     for(std::vector<Agent>::iterator agent = agents.begin(); agent != agents.end(); agent++) {
@@ -224,7 +226,8 @@ void AgentList::broadcastToAgents(char *broadcastData, size_t dataBytes,const ch
 }
 
 void AgentList::pingAgents() {
-    char payload[] = "P";
+    char payload[1];
+    *payload = PACKET_HEADER_PING;
     
     for(std::vector<Agent>::iterator agent = agents.begin(); agent != agents.end(); agent++) {
         if (agent->getType() == 'I') {

shared/src/AgentList.h

@@ -69,6 +69,7 @@ public:
     
 	static const char* AGENTS_OF_TYPE_HEAD;
 	static const char* AGENTS_OF_TYPE_VOXEL_AND_INTERFACE;
+	static const char* AGENTS_OF_TYPE_VOXEL;
     
 };
 

shared/src/AngleUtils.h

@@ -11,22 +11,22 @@
 
 #include <math.h>
 
-struct Degrees
+struct Degrees {
-{
+
     static float pi() { return 180.0f; }
     static float twicePi() { return 360.0f; }
     static float halfPi()  { return 90.0f; }
 };
 
-struct Radians
+struct Radians {
-{
+
     static float pi() { return 3.141592653589793f; }
     static float twicePi() { return 6.283185307179586f; }
     static float halfPi()  { return 1.5707963267948966; }
 };
 
-struct Rotations
+struct Rotations {
-{
+
     static float pi() { return 0.5f; }
     static float twicePi() { return 1.0f; }
     static float halfPi()  { return 0.25f; }
@@ -36,8 +36,8 @@ struct Rotations
  * Converts an angle from one unit to another.
  */
 template< class UnitFrom, class UnitTo >
-float angleConvert(float a)
+float angleConvert(float a) {
-{
+
     return a * (UnitTo::halfPi() / UnitFrom::halfPi());
 }
 
@@ -46,8 +46,8 @@ float angleConvert(float a)
  * Clamps an angle to the range of [-180; 180) degrees.
  */
 template< class Unit >
-float angleSignedNormal(float a)
+float angleSignedNormal(float a) {
-{
+
     float result = remainder(a, Unit::twicePi());
     if (result == Unit::pi())
         result = -Unit::pi();
@@ -58,8 +58,8 @@ float angleSignedNormal(float a)
  * Clamps an angle to the range of [0; 360) degrees.
  */
 template< class Unit >
-float angleUnsignedNormal(float a)
+float angleUnsignedNormal(float a) {
-{
+
     return angleSignedNormal<Unit>(a - Unit::pi()) + Unit::pi();
 }
 
@@ -72,16 +72,16 @@ float angleUnsignedNormal(float a)
  * Both poles can be reached from any azimuthal direction.
  */
 template< class Unit >
-void angleHorizontalPolar(float& azimuth, float& altitude)
+void angleHorizontalPolar(float& azimuth, float& altitude) {
-{
+
     altitude = angleSignedNormal<Unit>(altitude);
-    if (altitude > Unit::halfPi())
+    if (altitude > Unit::halfPi()) {
-    {
+
         altitude = Unit::pi() - altitude;
         azimuth += Unit::pi();
-    }
+
-    else if (altitude < -Unit::halfPi())
+    } else if (altitude < -Unit::halfPi()) {
-    {
+
         altitude = -Unit::pi() - altitude;
         azimuth += Unit::pi();
     }

shared/src/CounterStats.cpp

@@ -0,0 +1,90 @@
+//
+//  CounterStats.cpp
+//  hifi
+//
+//  Created by Brad Hefta-Gaub on 2013/04/08.
+//
+//  Poor-man's counter stats collector class. Useful for collecting running averages
+//  and other stats for countable things. 
+//
+//
+
+#include "CounterStats.h"
+#include <cstdio>
+#include <sys/time.h>
+#include <string>
+#include <map>
+
+
+//private:
+//	long int	currentCount;
+//  long int	currentDelta;
+//	double 		currentTime;
+//	double		totalTime;
+//	
+//	long int	countSamples[COUNTETSTATS_SAMPLES_TO_KEEP] = {};
+//	long int	deltaSamples[COUNTETSTATS_SAMPLES_TO_KEEP] = {};
+//	double		timeSamples[COUNTETSTATS_SAMPLES_TO_KEEP] = {};
+//  int			sampleAt;
+
+
+void CounterStatHistory::recordSample(long int thisCount) {
+	timeval now;
+	gettimeofday(&now,NULL);
+	double nowSeconds = (now.tv_usec/1000000.0)+(now.tv_sec);
+	this->recordSample(nowSeconds,thisCount);
+}
+
+void CounterStatHistory::recordSample(double thisTime, long int thisCount) {
+
+	// how much did we change since last sample?
+	long int thisDelta = thisCount - this->lastCount;
+	double elapsed = thisTime - this->lastTime;
+
+	// record the latest values
+	this->currentCount = thisCount;
+	this->currentTime = thisTime;
+	this->currentDelta = thisDelta;
+
+	//printf("CounterStatHistory[%s]::recordSample(thisTime %lf, thisCount= %ld)\n",this->name.c_str(),thisTime,thisCount);
+	
+	// if more than 1/10th of a second has passed, then record 
+	// things in our rolling history
+	if (elapsed > 0.1) {
+		this->lastTime = thisTime;
+		this->lastCount = thisCount;
+	
+		// record it in our history...
+		this->sampleAt = (this->sampleAt+1)%COUNTETSTATS_SAMPLES_TO_KEEP;
+		if (this->sampleCount<COUNTETSTATS_SAMPLES_TO_KEEP) {
+			this->sampleCount++;
+		}
+		this->countSamples[this->sampleAt]=thisCount;
+		this->timeSamples[this->sampleAt]=thisTime;
+		this->deltaSamples[this->sampleAt]=thisDelta;
+
+		//printf("CounterStatHistory[%s]::recordSample() ACTUALLY RECORDING IT sampleAt=%d thisTime %lf, thisCount= %ld)\n",this->name.c_str(),this->sampleAt,thisTime,thisCount);
+
+	}
+		
+}
+
+long int CounterStatHistory::getRunningAverage() {
+	// before we calculate our running average, always "reset" the current count, with the current time
+	// this will flush out old data, if we haven't been adding any new data.
+	this->recordSample(this->currentCount);
+
+	long int runningTotal = 0;
+	double minTime = this->timeSamples[0];
+	double maxTime = this->timeSamples[0];
+	
+	for (int i =0; i < this->sampleCount; i++) {
+		minTime = std::min(minTime,this->timeSamples[i]);
+		maxTime = std::max(maxTime,this->timeSamples[i]);
+		runningTotal += this->deltaSamples[i];
+	}
+	
+	double elapsedTime = maxTime-minTime;
+	long int runningAverage = runningTotal/elapsedTime;
+	return runningAverage;
+}

shared/src/CounterStats.h

@@ -0,0 +1,79 @@
+//
+//  CounterStats.h
+//  hifi
+//
+//  Created by Brad Hefta-Gaub on 3/29/13.
+//
+//  Poor-man's counter stats collector class. Useful for collecting running averages
+//  and other stats for countable things. 
+//
+//
+
+#ifndef __hifi__CounterStats__
+#define __hifi__CounterStats__
+
+#include <cstring>
+#include <string>
+#include <map>
+
+// TIME_FRAME should be SAMPLES_TO_KEEP * TIME_BETWEEN_SAMPLES
+#define COUNTETSTATS_SAMPLES_TO_KEEP 50
+#define COUNTETSTATS_TIME_BETWEEN_SAMPLES 0.1
+#define COUNTETSTATS_TIME_FRAME (COUNTETSTATS_SAMPLES_TO_KEEP*COUNTETSTATS_TIME_BETWEEN_SAMPLES)
+
+class CounterStatHistory {
+
+private:
+	long int	currentCount;
+	long int	currentDelta;
+	double 		currentTime;
+
+	long int	lastCount;
+	double 		lastTime;
+	
+	double		totalTime;
+	
+	long int	countSamples[COUNTETSTATS_SAMPLES_TO_KEEP];
+	long int	deltaSamples[COUNTETSTATS_SAMPLES_TO_KEEP];
+	double		timeSamples[COUNTETSTATS_SAMPLES_TO_KEEP];
+	int			sampleAt;
+	int			sampleCount;
+	
+public:
+	std::string name;
+
+	CounterStatHistory(std::string myName): 
+		currentCount(0), currentDelta(0),currentTime(0.0), 
+		lastCount(0),lastTime(0.0), 
+		totalTime(0.0), 
+		sampleAt(-1),sampleCount(0), name(myName) {};
+	
+	CounterStatHistory(): 
+		currentCount(0), currentDelta(0),currentTime(0.0), 
+		lastCount(0),lastTime(0.0), 
+		totalTime(0.0), 
+		sampleAt(-1),sampleCount(0) {};
+		
+	CounterStatHistory(std::string myName, double initialTime, long int initialCount) :
+        currentCount(initialCount), currentDelta(0), currentTime(initialTime), 
+		lastCount(initialCount),lastTime(initialTime), 
+        totalTime(initialTime), 
+        sampleAt(-1), sampleCount(0), name(myName) {};
+    
+	void recordSample(long int thisCount);
+	void recordSample(double thisTime, long int thisCount);
+	long int getRunningAverage();
+
+	long int getAverage() {
+		return currentCount/totalTime; 
+	};
+
+	double getTotalTime() {
+		return totalTime; 
+	};
+	long int getCount() {
+		return currentCount; 
+	};
+};
+
+#endif /* defined(__hifi__CounterStat__) */

shared/src/FloodFill.h

@@ -11,6 +11,29 @@
 
 /**
  * Line scanning, iterative flood fill algorithm.
+ *
+ * The strategy must obey the following contract:
+ *
+ *   There is an associated cursor that represents a position on the image. 
+ *   The member functions 'left(C&)', 'right(C&)', 'up(C&)', and 'down(C&)' 
+ *   move it.
+ *   The state of a cursor can be deferred to temporary storage (typically a
+ *   stack or a queue) using the 'defer(C const&)' member function. 
+ *   Calling 'deferred(C&)' restores a cursor's state from temporary storage
+ *   and removes it there.
+ *   The 'select(C const&)' and 'process(C const&)' functions control the 
+ *   algorithm. The former is called to determine where to go. It may be
+ *   called multiple times but does not have to (and should not) return 
+ *   'true' more than once for a pixel to be selected (will cause memory
+ *   overuse, otherwise). The latter will never be called for a given pixel
+ *   unless previously selected. It may be called multiple times, in which
+ *   case it should return 'true' upon successful processing and 'false'
+ *   when an already processed pixel has been visited. 
+ *
+ * Note: The terms "image" and "pixel" are used for illustratory purposes
+ * and mean "undirected graph with 4-connected 2D grid topology" and "node",
+ * respectively.
+ *
  */
 template< class Strategy, typename Cursor >
 void floodFill(Cursor const& position,
@@ -18,8 +41,8 @@ void floodFill(Cursor const& position,
 
 
 template< class Strategy, typename Cursor > 
-struct floodFill_impl : Strategy
+struct floodFill_impl : Strategy {
-{
+
     floodFill_impl(Strategy const& s) : Strategy(s) { }
 
     using Strategy::select;
@@ -33,16 +56,19 @@ struct floodFill_impl : Strategy
     using Strategy::defer;
     using Strategy::deferred;
 
-    void go(Cursor position)
+    void go(Cursor position) {
-    {
+
+        if (! select(position)) {
+            return;
+        }
+
         Cursor higher, lower, h,l, i;
         bool higherFound, lowerFound, hf, lf;
-        do
+        do {
-        {
-            if (! select(position))
-                continue;
 
-            process(position);
+            if (! process(position)) {
+                continue;
+            }
 
             higher = position; higherFound = false;
             up(higher); yTest(higher, higherFound);
@@ -51,33 +77,39 @@ struct floodFill_impl : Strategy
 
             i = position, h = higher, l = lower;
             hf = higherFound, lf = lowerFound;
-            do { right(i), right(h), right(l); yTest(h,hf); yTest(l,lf); }
+            do {
-            while (selectAndProcess(i));
+                right(i), right(h), right(l); yTest(h,hf); yTest(l,lf); 
+
+            } while (selectAndProcess(i));
 
             i = position, h = higher, l = lower;
             hf = higherFound, lf = lowerFound;
-            do { left(i); left(h); left(l); yTest(h,hf); yTest(l,lf); }
+            do {
-            while (selectAndProcess(i));
+                left(i); left(h); left(l); yTest(h,hf); yTest(l,lf);
-        }
+
-        while (deferred(position));
+            } while (selectAndProcess(i));
+
+        } while (deferred(position));
     }
 
-    bool selectAndProcess(Cursor const& i)
+    bool selectAndProcess(Cursor const& i) {
-    {
+
-        if (select(i))
+        if (select(i)) {
-        {
+
             process(i);
             return true;
         }
         return false;
     }
 
-    void yTest(Cursor const& i, bool& state)
+    void yTest(Cursor const& i, bool& state) {
-    {
+
-        if (! select(i))
+        if (! select(i)) {
+
             state = false;
-        else if (! state)
+
-        {
+        } else if (! state) {
+
             state = true;
             defer(i);
         }
@@ -85,8 +117,8 @@ struct floodFill_impl : Strategy
 };
 
 template< class Strategy, typename Cursor >
-void floodFill(Cursor const& p, Strategy const& s)
+void floodFill(Cursor const& p, Strategy const& s) {
-{
+
     floodFill_impl<Strategy,Cursor>(s).go(p);
 }
 

shared/src/PacketHeaders.h

@@ -0,0 +1,23 @@
+//
+//  PacketHeaders.h
+//  hifi
+//
+//  Created by Stephen Birarda on 4/8/13.
+//
+//
+
+#ifndef hifi_PacketHeaders_h
+#define hifi_PacketHeaders_h
+
+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/Radix2InplaceSort.h

@@ -25,30 +25,27 @@ void radix2InplaceSort( BidiIterator from, BidiIterator to,
 
 
 template< class Scanner, typename Iterator > 
-struct radix2InplaceSort_impl : Scanner
+struct radix2InplaceSort_impl : Scanner {
-{
+
     radix2InplaceSort_impl(Scanner const& s) : Scanner(s) { }
 
     using Scanner::advance;
     using Scanner::bit;
 
-    void go(Iterator& from, Iterator& to, typename Scanner::state_type s)
+    void go(Iterator& from, Iterator& to, typename Scanner::state_type s) {
-    {
+
         Iterator l(from), r(to);
         unsigned cl, cr;
 
         using std::swap;
 
-        for (;;)
+        while (true) {
-        {
             // scan from left for set bit
             for (cl = cr = 0u; l != r ; ++l, ++cl)
-                if (bit(*l, s))
+                if (bit(*l, s)) {
-                {
                     // scan from the right for unset bit
                     for (++cr; --r != l ;++cr)
-                        if (! bit(*r, s))
+                        if (! bit(*r, s)) {
-                        {
                             // swap, continue scanning from left
                             swap(*l, *r);
                             break;
@@ -58,22 +55,23 @@ struct radix2InplaceSort_impl : Scanner
                 }
 
             // on to the next digit, if any
-            if (! advance(s))
+            if (! advance(s)) {
                 return; 
+            }
 
             // recurse into smaller branch and prepare iterative
             // processing of the other
-            if (cl < cr)
+            if (cl < cr) {
-            {
+
                 if (cl > 1u) go(from, l, s);
                 else if (cr <= 1u)
                     return;
 
                 l = from = r;
                 r = to;
-            }
+
-            else
+            } else {
-            {
+
                 if (cr > 1u) go(r, to, s);
                 else if (cl <= 1u)
                     return;
@@ -87,8 +85,8 @@ struct radix2InplaceSort_impl : Scanner
 
 template< class Radix2Scanner, typename BidiIterator >
 void radix2InplaceSort( BidiIterator from, BidiIterator to,
-        Radix2Scanner const& scanner)
+        Radix2Scanner const& scanner) {
-{
+
     radix2InplaceSort_impl<Radix2Scanner, BidiIterator>(scanner)
         .go(from, to, scanner.initial_state());
 }

shared/src/Radix2IntegerScanner.h

@@ -12,8 +12,8 @@
 #include <stddef.h>
 #include <stdint.h>
 
-namespace type_traits // those are needed for the declaration, see below
+namespace type_traits { // those are needed for the declaration, see below
-{
+
     // Note: There are better / more generally appicable implementations 
     // in C++11, make_signed is missing in TR1 too - so I just use C++98
     // hacks that get the job done...
@@ -39,8 +39,8 @@ class Radix2IntegerScanner;
 
 
 template< typename UInt >
-class Radix2IntegerScanner< UInt, false >
+class Radix2IntegerScanner< UInt, false > {
-{
+
         UInt valMsb;
     public:
 
@@ -48,9 +48,8 @@ class Radix2IntegerScanner< UInt, false >
             : valMsb(~UInt(0) &~ (~UInt(0) >> 1)) { }
 
         explicit Radix2IntegerScanner(int bits)
-            : valMsb(UInt(1u) << (bits - 1))
+            : valMsb(UInt(1u) << (bits - 1)) {
-        { }
+        }
-
 
         typedef UInt state_type;
 
@@ -67,12 +66,12 @@ class Radix2IntegerScanner< Int, true >
     public:
 
         Radix2IntegerScanner()
-            : valMsb(~state_type(0u) &~ (~state_type(0u) >> 1))
+            : valMsb(~state_type(0u) &~ (~state_type(0u) >> 1)) {
-        { }
+        }
 
         explicit Radix2IntegerScanner(int bits)
-            : valMsb(state_type(1u) << (bits - 1))
+            : valMsb(state_type(1u) << (bits - 1)) {
-        { }
+        }
 
 
         typedef typename type_traits::make_unsigned<Int>::type state_type;

shared/src/SharedUtil.cpp

@@ -10,6 +10,7 @@
 #include <cstdio>
 #include <cstring>
 #include "SharedUtil.h"
+#include "OctalCode.h"
 
 #ifdef __APPLE__
 #include <CoreFoundation/CoreFoundation.h>
@@ -124,6 +125,70 @@ bool cmdOptionExists(int argc, const char * argv[],const char* option) {
     return false;
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////
+// Function:    createVoxelEditMessage()
+// Description: creates an "insert" or "remove" voxel message for a voxel code 
+//              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 '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.
+//
+// HACK ATTACK: Well, what if this is larger than the MTU? That's the caller's problem, we
+//              just truncate the message
+// Usage:       
+//                  unsigned char* voxelData = pointToVoxel(x,y,z,s,red,green,blue);
+//                  tree->readCodeColorBufferToTree(voxelData);
+//                  delete voxelData;
+//
+// Complaints:  Brad :)
+#define GUESS_OF_VOXELCODE_SIZE 10
+#define MAXIMUM_EDIT_VOXEL_MESSAGE_SIZE 1500
+#define SIZE_OF_COLOR_DATA 3
+bool createVoxelEditMessage(unsigned char command, short int sequence, 
+        int voxelCount, VoxelDetail* voxelDetails, unsigned char*& bufferOut, int& sizeOut) {
+        
+    bool success = true; // assume the best
+    int messageSize = MAXIMUM_EDIT_VOXEL_MESSAGE_SIZE; // just a guess for now
+    int actualMessageSize = 3;
+    unsigned char* messageBuffer = new unsigned char[messageSize];
+    unsigned short int* sequenceAt = (unsigned short int*)&messageBuffer[1];
+    
+    messageBuffer[0]=command;
+    *sequenceAt=sequence;
+    unsigned char* copyAt = &messageBuffer[3];
+
+    for (int i=0;i<voxelCount && success;i++) {
+        // get the coded voxel
+        unsigned char* voxelData = pointToVoxel(voxelDetails[i].x,voxelDetails[i].y,voxelDetails[i].z,
+            voxelDetails[i].s,voxelDetails[i].red,voxelDetails[i].green,voxelDetails[i].blue);
+            
+        int lengthOfVoxelData = bytesRequiredForCodeLength(*voxelData)+SIZE_OF_COLOR_DATA;
+        
+        // make sure we have room to copy this voxel
+        if (actualMessageSize+lengthOfVoxelData > MAXIMUM_EDIT_VOXEL_MESSAGE_SIZE) {
+            success=false;
+        } else {
+            // add it to our message
+            memcpy(copyAt,voxelData,lengthOfVoxelData);
+            copyAt+=lengthOfVoxelData+SIZE_OF_COLOR_DATA;
+            actualMessageSize+=lengthOfVoxelData+SIZE_OF_COLOR_DATA;
+        }
+        // cleanup
+        delete voxelData;
+    }
+
+    if (success) {    
+        // finally, copy the result to the output
+        bufferOut = new unsigned char[actualMessageSize];
+        sizeOut=actualMessageSize;
+        memcpy(bufferOut,messageBuffer,actualMessageSize);
+    }
+    return success;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////
 // Function:    pointToVoxel()
 // Description: Given a universal point with location x,y,z this will return the voxel

shared/src/SharedUtil.h

@@ -35,6 +35,19 @@ void switchToResourcesIfRequired();
 
 const char* getCmdOption(int argc, const char * argv[],const char* option);
 bool cmdOptionExists(int argc, const char * argv[],const char* option);
+
+struct VoxelDetail {
+	float x;
+	float y;
+	float z;
+	float s;
+	unsigned char red;
+	unsigned char green;
+	unsigned char blue;
+};
+
 unsigned char* pointToVoxel(float x, float y, float z, float s, unsigned char r, unsigned char g, unsigned char b );
+bool createVoxelEditMessage(unsigned char command, short int sequence, 
+        int voxelCount, VoxelDetail* voxelDetails, unsigned char*& bufferOut, int& sizeOut);
 
 #endif /* defined(__hifi__SharedUtil__) */

shared/src/UrlReader.cpp

@@ -23,8 +23,8 @@ char const* const UrlReader::error_leftover_input   = "UrlReader: Incomplete pro
 #define hnd_curl static_cast<CURL*>(_ptrImpl)
 
 UrlReader::UrlReader()
-    : _ptrImpl(0l), _arrXtra(0l), _strError(0l)
+    : _ptrImpl(0l), _arrXtra(0l), _strError(0l) {
-{
+
     _arrXtra = new(std::nothrow) char[max_read_ahead];
     if (! _arrXtra) { _strError = error_init_failed; return; }
     _ptrImpl =  curl_easy_init();
@@ -34,15 +34,15 @@ UrlReader::UrlReader()
     curl_easy_setopt(hnd_curl, CURLOPT_FILETIME, 1l);
 }
 
-UrlReader::~UrlReader()
+UrlReader::~UrlReader() {
-{
+
     delete _arrXtra;
     if (! hnd_curl) return;
     curl_easy_cleanup(hnd_curl);
 }
 
-bool UrlReader::perform(char const* url, transfer_callback* cb)
+bool UrlReader::perform(char const* url, transfer_callback* cb) {
-{
+
     curl_easy_setopt(hnd_curl, CURLOPT_URL, url);
     curl_easy_setopt(hnd_curl, CURLOPT_WRITEFUNCTION, cb);
     curl_easy_setopt(hnd_curl, CURLOPT_WRITEDATA, this);
@@ -61,8 +61,8 @@ bool UrlReader::perform(char const* url, transfer_callback* cb)
 }
 
 void UrlReader::getinfo(char const*& url,
-        char const*& type, int64_t& length, int64_t& stardate)
+        char const*& type, int64_t& length, int64_t& stardate) {
-{
+
     curl_easy_getinfo(hnd_curl, CURLINFO_EFFECTIVE_URL, & url);
     curl_easy_getinfo(hnd_curl, CURLINFO_CONTENT_TYPE, & type);
 

shared/src/UrlReader.h

@@ -17,8 +17,8 @@
  * UrlReader class that encapsulates a context for sequential data retrieval
  * via URLs. Use one per thread.
  */
-class UrlReader
+class UrlReader {
-{
+
         void*       _ptrImpl;
         char*       _arrXtra;
         char const* _strError;
@@ -149,8 +149,7 @@ class UrlReader
 };
 
 template< class ContentStream >
-bool UrlReader::readUrl(char const* url, ContentStream& s)
+bool UrlReader::readUrl(char const* url, ContentStream& s) {
-{
     if (! _ptrImpl) return false;
     _strError = success;
     _ptrStream = & s;
@@ -162,24 +161,24 @@ bool UrlReader::readUrl(char const* url, ContentStream& s)
 
 inline char const* UrlReader::getError() const { return this->_strError; }
 
-inline void UrlReader::setError(char const* static_c_string)
+inline void UrlReader::setError(char const* static_c_string) {
-{
+
     if (this->_strError == success)
         this->_strError = static_c_string;
 }
 
 template< class Stream >
 size_t UrlReader::callback_template(
-        char *input, size_t size, size_t nmemb, void* thiz)
+        char *input, size_t size, size_t nmemb, void* thiz) {
-{
+
     size *= nmemb;
 
     UrlReader* me = static_cast<UrlReader*>(thiz);
     Stream* stream = static_cast<Stream*>(me->_ptrStream);
 
     // first call?
-    if (me->_valXtraSize == ~size_t(0))
+    if (me->_valXtraSize == ~size_t(0)) {
-    {
+
         me->_valXtraSize = 0u;
         // extract meta information and call 'begin'
         char const* url, * type;
@@ -190,14 +189,14 @@ size_t UrlReader::callback_template(
 
     size_t input_offset = 0u;
 
-    for (;;)
+    while (true) {
-    {
+
         char* buffer = input + input_offset;
         size_t bytes = size - input_offset;
 
         // data in extra buffer?
-        if (me->_valXtraSize > 0)
+        if (me->_valXtraSize > 0) {
-        {
+
             // fill extra buffer with beginning of input
             size_t fill = max_read_ahead - me->_valXtraSize;
             if (bytes < fill) fill = bytes;
@@ -210,36 +209,36 @@ size_t UrlReader::callback_template(
 
         // call 'transfer'
         size_t processed = stream->transfer(buffer, bytes);
-        if (processed == abort)
+        if (processed == abort) {
-        {
+
             me->setError(error_aborted);
             return 0u;
-        }
+
-        else if (! processed && ! input)
+        } else if (! processed && ! input) {
-        {
+
             me->setError(error_leftover_input);
             return 0u;
         }
         size_t unprocessed = bytes - processed;
 
         // can switch to input buffer, now?
-        if (buffer == me->_arrXtra && unprocessed <= input_offset)
+        if (buffer == me->_arrXtra && unprocessed <= input_offset) {
-        {
+
             me->_valXtraSize = 0u;
             input_offset -= unprocessed;
-        }
+
-        else // no? unprocessed data -> extra buffer
+        } else { // no? unprocessed data -> extra buffer
-        {
+
-            if (unprocessed > max_read_ahead)
+            if (unprocessed > max_read_ahead) {
-            {
+
                 me->setError(error_buffer_overflow);
                 return 0;
             }
             me->_valXtraSize = unprocessed;
             memmove(me->_arrXtra, buffer + processed, unprocessed);
 
-            if (input_offset == size || buffer != me->_arrXtra)
+            if (input_offset == size || buffer != me->_arrXtra) {
-            {
+
                 return size;
             }
         }

shared/src/VoxelNode.cpp

@@ -37,11 +37,8 @@ void VoxelNode::addChildAtIndex(int childIndex) {
 }
 
 // will average the child colors...
-void VoxelNode::setColorFromAverageOfChildren(int * colorArray) {
+void VoxelNode::setColorFromAverageOfChildren() {
-    if (colorArray == NULL) {
+	int colorArray[4] = {0,0,0,0};
-        colorArray = new int[4];
-        memset(colorArray, 0, 4*sizeof(int));
-        
 	for (int i = 0; i < 8; i++) {
 		if (children[i] != NULL && children[i]->color[3] == 1) {
 			for (int j = 0; j < 3; j++) {
@@ -50,12 +47,10 @@ void VoxelNode::setColorFromAverageOfChildren(int * colorArray) {
 			colorArray[3]++;
 		}
 	}
-    }
     
     if (colorArray[3] > 4) {
         // we need at least 4 colored children to have an average color value
         // or if we have none we generate random values
-        
         for (int c = 0; c < 3; c++) {
             // set the average color value
             color[c] = colorArray[c] / colorArray[3];

shared/src/VoxelNode.h

@@ -17,7 +17,7 @@ public:
     ~VoxelNode();
     
     void addChildAtIndex(int childIndex);
-    void setColorFromAverageOfChildren(int * colorArray = NULL);
+    void setColorFromAverageOfChildren();
     void setRandomColor(int minimumBrightness);
     bool collapseIdenticalLeaves();
     

shared/src/VoxelTree.cpp

@@ -10,6 +10,8 @@
 #include <cstdio>
 #include <cmath>
 #include "SharedUtil.h"
+#include "PacketHeaders.h"
+#include "CounterStats.h"
 #include "OctalCode.h"
 #include "VoxelTree.h"
 #include <iostream> // to load voxels from file
@@ -44,6 +46,15 @@ VoxelTree::VoxelTree() {
     rootNode = new VoxelNode();
     rootNode->octalCode = new unsigned char[1];
     *rootNode->octalCode = 0;
+
+	// Some stats tracking    
+	this->voxelsCreated = 0; // when a voxel is created in the tree (object new'd)
+	this->voxelsColored = 0; // when a voxel is colored/set in the tree (object may have already existed)
+	this->voxelsBytesRead = 0;
+	voxelsCreatedStats.name = "voxelsCreated";
+	voxelsColoredStats.name = "voxelsColored";
+	voxelsBytesReadStats.name = "voxelsBytesRead";
+
 }
 
 VoxelTree::~VoxelTree() {
@@ -94,14 +105,15 @@ VoxelNode * VoxelTree::createMissingNode(VoxelNode *lastParentNode, unsigned cha
     }
 }
 
+// BHG Notes: We appear to call this function for every Voxel Node getting created.
+// This is recursive in nature. So, for example, if we are given an octal code for
+// a 1/256th size voxel, we appear to call this function 8 times. Maybe??
 int VoxelTree::readNodeData(VoxelNode *destinationNode,
                             unsigned char * nodeData,
                             int bytesLeftToRead) {
     
     // instantiate variable for bytes already read
     int bytesRead = 1;
-    int colorArray[4] = {};
-    
     for (int i = 0; i < 8; i++) {
         // check the colors mask to see if we have a child to color in
         if (oneAtBit(*nodeData, i)) {
@@ -109,23 +121,22 @@ int VoxelTree::readNodeData(VoxelNode *destinationNode,
             // create the child if it doesn't exist
             if (destinationNode->children[i] == NULL) {
                 destinationNode->addChildAtIndex(i);
+                this->voxelsCreated++;
+                this->voxelsCreatedStats.recordSample(this->voxelsCreated);
             }
             
             // pull the color for this child
             memcpy(destinationNode->children[i]->color, nodeData + bytesRead, 3);
             destinationNode->children[i]->color[3] = 1;
-           
+			this->voxelsColored++;
-            for (int j = 0; j < 3; j++) {
+			this->voxelsColoredStats.recordSample(this->voxelsColored);
-                colorArray[j] += destinationNode->children[i]->color[j];
-            }
            
             bytesRead += 3;
-            colorArray[3]++;
         }
     }
     
     // average node's color based on color of children
-    destinationNode->setColorFromAverageOfChildren(colorArray);
+    destinationNode->setColorFromAverageOfChildren();
     
     // give this destination node the child mask from the packet
     unsigned char childMask = *(nodeData + bytesRead);
@@ -140,6 +151,8 @@ int VoxelTree::readNodeData(VoxelNode *destinationNode,
             if (destinationNode->children[childIndex] == NULL) {
                 // add a child at that index, if it doesn't exist
                 destinationNode->addChildAtIndex(childIndex);
+                this->voxelsCreated++;
+                this->voxelsCreatedStats.recordSample(this->voxelsCreated);
             }
             
             // tell the child to read the subsequent data
@@ -165,6 +178,9 @@ void VoxelTree::readBitstreamToTree(unsigned char * bitstream, int bufferSizeByt
     
     int octalCodeBytes = bytesRequiredForCodeLength(*bitstream);
     readNodeData(bitstreamRootNode, bitstream + octalCodeBytes, bufferSizeBytes - octalCodeBytes);
+    
+    this->voxelsBytesRead += bufferSizeBytes;
+	this->voxelsBytesReadStats.recordSample(this->voxelsBytesRead);
 }
 
 // Note: uses the codeColorBuffer format, but the color's are ignored, because
@@ -209,6 +225,16 @@ void VoxelTree::deleteVoxelCodeFromTree(unsigned char *codeBuffer) {
     }
 }
 
+void VoxelTree::eraseAllVoxels() {
+
+	// XXXBHG Hack attack - is there a better way to erase the voxel tree?
+
+	delete rootNode; // this will recurse and delete all children
+	rootNode = new VoxelNode();
+	rootNode->octalCode = new unsigned char[1];
+	*rootNode->octalCode = 0;
+}
+
 void VoxelTree::readCodeColorBufferToTree(unsigned char *codeColorBuffer) {
     VoxelNode *lastCreatedNode = nodeForOctalCode(rootNode, codeColorBuffer, NULL);
     
@@ -256,9 +282,16 @@ unsigned char * VoxelTree::loadBitstreamBuffer(unsigned char *& bitstreamBuffer,
         
         float halfUnitForVoxel = powf(0.5, *currentVoxelNode->octalCode) * (0.5 * TREE_SCALE);
 
-        float distanceToVoxelCenter = sqrtf(powf(agentPosition[0] - thisNodePosition[0] - halfUnitForVoxel, 2) +
+		// XXXBHG - Note: It appears as if the X and Z coordinates of Head or Agent are flip-flopped relative to the 
+		// coords of the voxel space. This flip flop causes LOD behavior to be extremely odd. This is my temporary hack 
+		// to fix this behavior. To disable this swap, set swapXandZ to false.
+        bool swapXandZ=true;
+        float agentX = swapXandZ ? agentPosition[2] : agentPosition[0];
+        float agentZ = swapXandZ ? agentPosition[0] : agentPosition[2];
+        
+        float distanceToVoxelCenter = sqrtf(powf(agentX - thisNodePosition[0] - halfUnitForVoxel, 2) +
                                             powf(agentPosition[1] - thisNodePosition[1] - halfUnitForVoxel, 2) +
-                                            powf(agentPosition[2] - thisNodePosition[2] - halfUnitForVoxel, 2));
+                                            powf(agentZ - thisNodePosition[2] - halfUnitForVoxel, 2));
         
         // if the distance to this voxel's center is less than the threshold
         // distance for its children, we should send the children
@@ -277,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);

shared/src/VoxelTree.h

@@ -9,6 +9,8 @@
 #ifndef __hifi__VoxelTree__
 #define __hifi__VoxelTree__
 
+#include "CounterStats.h"
+
 #include "VoxelNode.h"
 #include "MarkerNode.h"
 
@@ -20,13 +22,25 @@ class VoxelTree {
     VoxelNode * nodeForOctalCode(VoxelNode *ancestorNode, unsigned char * needleCode, VoxelNode** parentOfFoundNode);
     VoxelNode * createMissingNode(VoxelNode *lastParentNode, unsigned char *deepestCodeToCreate);
     int readNodeData(VoxelNode *destinationNode, unsigned char * nodeData, int bufferSizeBytes);
+
+
 public:
+	long int voxelsCreated;
+	long int voxelsColored;
+	long int voxelsBytesRead;
+	
+	CounterStatHistory voxelsCreatedStats;
+	CounterStatHistory voxelsColoredStats;
+	CounterStatHistory voxelsBytesReadStats;
+
     VoxelTree();
     ~VoxelTree();
     
     VoxelNode *rootNode;
     int leavesWrittenToBitstream;
 
+	void eraseAllVoxels();
+
 	void processRemoveVoxelBitstream(unsigned char * bitstream, int bufferSizeBytes);
     void readBitstreamToTree(unsigned char * bitstream, int bufferSizeBytes);
     void readCodeColorBufferToTree(unsigned char *codeColorBuffer);

tools/sendvoxels.php

@@ -38,31 +38,53 @@ function send_voxels($inputFileName,$server,$port,$command) {
 	socket_close($socketHandle);
 }
 
+function send_zcommand($server,$port,$command) {
+	$socketHandle = socket_create(AF_INET, SOCK_DGRAM, SOL_UDP);
+	$serverIP = $server; 
+	$serverSendPort = $port;
+
+	// [1:'Z'][2:command][0]...
+	$netData = pack("c",ord('Z'));
+	$netData .= $command;
+	$netData .= pack("c",0);
+
+	$packetSize = 2+strlen($command);
+	echo "sending packet server=$serverIP port=$serverSendPort size=$packetSize \n";
+	$result = socket_sendto($socketHandle, $netData, $packetSize, 0, $serverIP, $serverSendPort);
+	socket_close($socketHandle);
+}
+
 function testmode_send_voxels($server,$port) {
 	echo "psych! test mode not implemented!\n";
 }
 
-$options = getopt("i:s:p:c:",array('testmode'));
+$options = getopt("i:s:p:c:",array('testmode','zcommand:'));
+//print_r($options);
+//break;
 
-if (empty($options['i']) || empty($options['i'])) {
+if (empty($options['i']) && empty($options['zcommand'])) {
 	echo "USAGE: sendvoxels.php -i 'inputFileName' -s [serverip] -p [port] -c [I|R] \n";
 } else {
 	$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']) {
 		echo "TEST MODE Sending Voxels server:$server port:$port \n";
 		testmode_send_voxels($server,$port);
+	} else if ($options['zcommand'] && $command=='Z') {
+		echo "Sending Z command to server:$server port:$port \n";
+		send_zcommand($server,$port,$options['zcommand']);
 	} else {
 		echo "Sending Voxels file:$filename server:$server port:$port command:$command \n";
 		send_voxels($filename,$server,$port,$command);

voxel/src/main.cpp

@@ -16,6 +16,7 @@
 #include <VoxelTree.h>
 #include "VoxelAgentData.h"
 #include <SharedUtil.h>
+#include <PacketHeaders.h>
 
 #ifdef _WIN32
 #include "Syssocket.h"
@@ -47,6 +48,8 @@ const int MAX_VOXEL_TREE_DEPTH_LEVELS = 4;
 AgentList agentList('V', VOXEL_LISTEN_PORT);
 VoxelTree randomTree;
 
+bool wantColorRandomizer = false;
+
 void addSphere(VoxelTree * tree,bool random, bool wantColorRandomizer) {
 	float r  = random ? randFloatInRange(0.05,0.1) : 0.25;
 	float xc = random ? randFloatInRange(r,(1-r)) : 0.5;
@@ -83,8 +86,6 @@ void randomlyFillVoxelTree(int levelsToGo, VoxelNode *currentRootNode) {
     if (levelsToGo > 0) {
 
         bool createdChildren = false;
-        int colorArray[4] = {};
-        
         createdChildren = false;
         
         for (int i = 0; i < 8; i++) {
@@ -96,15 +97,6 @@ void randomlyFillVoxelTree(int levelsToGo, VoxelNode *currentRootNode) {
                 currentRootNode->children[i]->octalCode = childOctalCode(currentRootNode->octalCode, i);
 
                 randomlyFillVoxelTree(levelsToGo - 1, currentRootNode->children[i]);
-
-                if (currentRootNode->children[i]->color[3] == 1) {
-                    for (int c = 0; c < 3; c++) {
-                        colorArray[c] += currentRootNode->children[i]->color[c];
-                    }
-                    
-                    colorArray[3]++;
-                }
-                
                 createdChildren = true;
             }
         }
@@ -115,7 +107,7 @@ void randomlyFillVoxelTree(int levelsToGo, VoxelNode *currentRootNode) {
             currentRootNode->setRandomColor(MIN_BRIGHTNESS);
         } else {
             // set the color value for this node
-            currentRootNode->setColorFromAverageOfChildren(colorArray);
+            currentRootNode->setColorFromAverageOfChildren();
         }
     } else {
         // this is a leaf node, just give it a color
@@ -123,6 +115,33 @@ void randomlyFillVoxelTree(int levelsToGo, VoxelNode *currentRootNode) {
     }
 }
 
+void eraseVoxelTreeAndCleanupAgentVisitData() {
+
+	// As our tree to erase all it's voxels
+	::randomTree.eraseAllVoxels();
+
+	// enumerate the agents clean up their marker nodes
+	for (int i = 0; i < agentList.getAgents().size(); i++) {
+
+		//printf("eraseVoxelTreeAndCleanupAgentVisitData() agent[%d]\n",i);
+            
+		Agent *thisAgent = (Agent *)&::agentList.getAgents()[i];
+		VoxelAgentData *agentData = (VoxelAgentData *)(thisAgent->getLinkedData());
+
+		// lock this agent's delete mutex so that the delete thread doesn't
+		// kill the agent while we are working with it
+		pthread_mutex_lock(&thisAgent->deleteMutex);
+
+		// clean up the agent visit data
+		delete agentData->rootMarkerNode;
+		agentData->rootMarkerNode = new MarkerNode();
+		
+		// unlock the delete mutex so the other thread can
+		// kill the agent if it has dissapeared
+		pthread_mutex_unlock(&thisAgent->deleteMutex);
+	}
+}
+
 void *distributeVoxelsToListeners(void *args) {
     
     timeval lastSendTime;
@@ -169,7 +188,12 @@ void *distributeVoxelsToListeners(void *args) {
                 packetCount++;
                 totalBytesSent += voxelPacketEnd - voxelPacket;
                 
-                if (agentData->rootMarkerNode->childrenVisitedMask == 255) {
+                // XXXBHG Hack Attack: This is temporary code to help debug an issue.
+                // Normally we use this break to prevent resending voxels that an agent has
+                // already visited. But since we might be modifying the voxel tree we might
+                // want to always send. This is a hack to test the behavior
+                bool alwaysSend = true;
+                if (!alwaysSend && agentData->rootMarkerNode->childrenVisitedMask == 255) {
                     break;
                 }
             }
@@ -229,7 +253,7 @@ int main(int argc, const char * argv[])
 	// Voxel File. If so, load it now.
 	const char* WANT_COLOR_RANDOMIZER="--WantColorRandomizer";
 	const char* INPUT_FILE="-i";
-    bool wantColorRandomizer = cmdOptionExists(argc, argv, WANT_COLOR_RANDOMIZER);
+    ::wantColorRandomizer = cmdOptionExists(argc, argv, WANT_COLOR_RANDOMIZER);
 
 	printf("wantColorRandomizer=%s\n",(wantColorRandomizer?"yes":"no"));
     const char* voxelsFilename = getCmdOption(argc, argv, INPUT_FILE);
@@ -270,31 +294,52 @@ 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);
+            	printf("got I - insert voxels - command from client receivedBytes=%ld itemNumber=%d\n",
+            		receivedBytes,itemNumber);
             	int atByte = 3;
             	unsigned char* pVoxelData = (unsigned char*)&packetData[3];
             	while (atByte < receivedBytes) {
             		unsigned char octets = (unsigned char)*pVoxelData;
             		int voxelDataSize = bytesRequiredForCodeLength(octets)+3; // 3 for color!
+            		int voxelCodeSize = bytesRequiredForCodeLength(octets);
+
+					// color randomization on insert
+					int colorRandomizer = ::wantColorRandomizer ? randIntInRange (-50, 50) : 0;
+					int red   = pVoxelData[voxelCodeSize+0];
+					int green = pVoxelData[voxelCodeSize+1];
+					int blue  = pVoxelData[voxelCodeSize+2];
+            		printf("insert voxels - wantColorRandomizer=%s old r=%d,g=%d,b=%d \n",
+            			(::wantColorRandomizer?"yes":"no"),red,green,blue);
+					red   = std::max(0,std::min(255,red   + colorRandomizer));
+					green = std::max(0,std::min(255,green + colorRandomizer));
+					blue  = std::max(0,std::min(255,blue  + colorRandomizer));
+            		printf("insert voxels - wantColorRandomizer=%s NEW r=%d,g=%d,b=%d \n",
+            			(::wantColorRandomizer?"yes":"no"),red,green,blue);
+					pVoxelData[voxelCodeSize+0]=red;
+					pVoxelData[voxelCodeSize+1]=green;
+					pVoxelData[voxelCodeSize+2]=blue;
 
             		float* vertices = firstVertexForCode(pVoxelData);
             		printf("inserting voxel at: %f,%f,%f\n",vertices[0],vertices[1],vertices[2]);
             		delete []vertices;
             		
 		            randomTree.readCodeColorBufferToTree(pVoxelData);
-	            	//printf("readCodeColorBufferToTree() of size=%d  atByte=%d receivedBytes=%ld\n",voxelDataSize,atByte,receivedBytes);
+	            	//printf("readCodeColorBufferToTree() of size=%d  atByte=%d receivedBytes=%ld\n",
+	            	//		voxelDataSize,atByte,receivedBytes);
             		// skip to next
             		pVoxelData+=voxelDataSize;
             		atByte+=voxelDataSize;
             	}
+            	// 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
@@ -302,7 +347,34 @@ int main(int argc, const char * argv[])
 				agentList.broadcastToAgents(packetData,receivedBytes,AgentList::AGENTS_OF_TYPE_HEAD);
             	
             }
-            if (packetData[0] == 'H') {
+            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
+				char* command = &packetData[1]; // start of the command
+				int commandLength = strlen(command); // commands are null terminated strings
+                int totalLength = 1+commandLength+1;
+
+				printf("got Z message len(%ld)= %s\n",receivedBytes,command);
+
+				while (totalLength <= receivedBytes) {
+					if (0==strcmp(command,(char*)"erase all")) {
+						printf("got Z message == erase all\n");
+						
+						eraseVoxelTreeAndCleanupAgentVisitData();
+					}
+					if (0==strcmp(command,(char*)"add scene")) {
+						printf("got Z message == add scene\n");
+						addSphereScene(&randomTree,false);
+					}
+                    totalLength += commandLength+1;
+				}
+
+				// 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);
+            }
+            if (packetData[0] == PACKET_HEADER_HEAD_DATA) {
                 if (agentList.addOrUpdateAgent(&agentPublicAddress,
                                                &agentPublicAddress,
                                                packetData[0],