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

audio-mixer/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8)
 
-set(ROOT_DIR ../)
+set(ROOT_DIR ..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
 set(TARGET_NAME audio-mixer)

audio-mixer/src/main.cpp

@@ -309,7 +309,7 @@ int main(int argc, const char * argv[])
                         agentList->increaseAgentId();
                     }
                     
-                    agentList->updateAgentWithData(agentAddress, (void *)packetData, receivedBytes);
+                    agentList->updateAgentWithData(agentAddress, packetData, receivedBytes);
                 } else {
                     memcpy(loopbackAudioPacket, packetData + 1 + (sizeof(float) * 4), 1024);
                     agentList->getAgentSocket().send(agentAddress, loopbackAudioPacket, 1024);

avatar-mixer/CMakeLists.txt

@@ -2,17 +2,21 @@ cmake_minimum_required(VERSION 2.8)
 
 set(TARGET_NAME "avatar-mixer")
 
-set(ROOT_DIR ../)
+set(ROOT_DIR ..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
+# setup for find modules
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../cmake/modules/")
+
 # setup the project
 include(${MACRO_DIR}/SetupHifiProject.cmake)
 setup_hifi_project(${TARGET_NAME})
 
-# link the shared hifi library
+# include glm
+include(${MACRO_DIR}/IncludeGLM.cmake)
+include_glm(${TARGET_NAME} ${ROOT_DIR})
+
+# link required hifi libraries
 include(${MACRO_DIR}/LinkHifiLibrary.cmake)
 link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR})
-
+link_hifi_library(avatars ${TARGET_NAME} ${ROOT_DIR})
-# link the threads library
-find_package(Threads REQUIRED)
-target_link_libraries(${TARGET_NAME} ${CMAKE_THREAD_LIBS_INIT})

avatar-mixer/src/AvatarAgentData.cpp

@@ -1,118 +0,0 @@
-//
-//  AvatarAgentData.cpp
-//  hifi
-//
-//  Created by Stephen Birarda on 4/9/13.
-//
-//
-
-#include <cstdio>
-
-#include "AvatarAgentData.h"
-
-AvatarAgentData::AvatarAgentData() {
-    
-}
-
-AvatarAgentData::~AvatarAgentData() {
-    
-}
-
-AvatarAgentData* AvatarAgentData::clone() const {
-    return new AvatarAgentData(*this);
-}
-
-void AvatarAgentData::parseData(void *data, int size) {
-    char* packetData = (char *)data + 1;
-    
-    // Extract data from packet
-    sscanf(packetData,
-           PACKET_FORMAT,
-           &_pitch,
-           &_yaw,
-           &_roll,
-           &_headPositionX,
-           &_headPositionY,
-           &_headPositionZ,
-           &_loudness,
-           &_averageLoudness,
-           &_handPositionX,
-           &_handPositionY,
-           &_handPositionZ);
-}
-
-float AvatarAgentData::getPitch() {
-    return _pitch;
-}
-
-float AvatarAgentData::getYaw() {
-    return _yaw;
-}
-
-float AvatarAgentData::getRoll() {
-    return _roll;
-}
-
-float AvatarAgentData::getHeadPositionX() {
-    return _headPositionX;
-}
-
-float AvatarAgentData::getHeadPositionY() {
-    return _headPositionY;
-}
-
-float AvatarAgentData::getHeadPositionZ() {
-    return _headPositionZ;
-}
-
-float AvatarAgentData::getLoudness() {
-    return _loudness;
-}
-
-float AvatarAgentData::getAverageLoudness() {
-    return _averageLoudness;
-}
-
-float AvatarAgentData::getHandPositionX() {
-    return _handPositionX;
-}
-
-float AvatarAgentData::getHandPositionY() {
-    return _handPositionY;
-}
-
-float AvatarAgentData::getHandPositionZ() {
-    return _handPositionZ;
-}
-
-void AvatarAgentData::setPitch(float pitch) {
-    _pitch = pitch;
-}
-
-void AvatarAgentData::setYaw(float yaw) {
-    _yaw = yaw;
-}
-
-void AvatarAgentData::setRoll(float roll) {
-    _roll = roll;
-}
-
-void AvatarAgentData::setHeadPosition(float x, float y, float z) {
-    _headPositionX = x;
-    _headPositionY = y;
-    _headPositionZ = z;
-}
-
-void AvatarAgentData::setLoudness(float loudness) {
-    _loudness = loudness;
-}
-
-void AvatarAgentData::setAverageLoudness(float averageLoudness) {
-    _averageLoudness = averageLoudness;
-}
-
-void AvatarAgentData::setHandPosition(float x, float y, float z) {
-    _handPositionX = x;
-    _handPositionY = y;
-    _handPositionZ = z;
-}

avatar-mixer/src/AvatarAgentData.h

@@ -1,58 +0,0 @@
-//
-//  AvatarAgentData.h
-//  hifi
-//
-//  Created by Stephen Birarda on 4/9/13.
-//
-//
-
-#ifndef __hifi__AvatarAgentData__
-#define __hifi__AvatarAgentData__
-
-#include <iostream>
-#include <AgentData.h>
-
-const char PACKET_FORMAT[] = "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f";
-
-class AvatarAgentData : public AgentData {
-public:
-    AvatarAgentData();
-    ~AvatarAgentData();
-    
-    void parseData(void *data, int size);
-    AvatarAgentData* clone() const;
-    
-    float getPitch();
-    void setPitch(float pitch);
-    float getYaw();
-    void setYaw(float yaw);
-    float getRoll();
-    void setRoll(float roll);
-    float getHeadPositionX();
-    float getHeadPositionY();
-    float getHeadPositionZ();
-    void setHeadPosition(float x, float y, float z);
-    float getLoudness();
-    void setLoudness(float loudness);
-    float getAverageLoudness();
-    void setAverageLoudness(float averageLoudness);
-    float getHandPositionX();
-    float getHandPositionY();
-    float getHandPositionZ();
-    void setHandPosition(float x, float y, float z);
-
-private:
-    float _pitch;
-    float _yaw;
-    float _roll;
-    float _headPositionX;
-    float _headPositionY;
-    float _headPositionZ;
-    float _loudness;
-    float _averageLoudness;
-    float _handPositionX;
-    float _handPositionY;
-    float _handPositionZ;
-};
-
-#endif /* defined(__hifi__AvatarAgentData__) */

avatar-mixer/src/main.cpp

@@ -32,37 +32,22 @@
 #include <StdDev.h>
 #include <UDPSocket.h>
 
-#include "AvatarAgentData.h"
+#include "AvatarData.h"
 
 const int AVATAR_LISTEN_PORT = 55444;
-const unsigned short BROADCAST_INTERVAL_USECS = 20 * 1000 * 1000;
 
 unsigned char *addAgentToBroadcastPacket(unsigned char *currentPosition, Agent *agentToAdd) {
     currentPosition += packAgentId(currentPosition, agentToAdd->getAgentId());
 
-    AvatarAgentData *agentData = (AvatarAgentData *)agentToAdd->getLinkedData();
+    AvatarData *agentData = (AvatarData *)agentToAdd->getLinkedData();
+    currentPosition += agentData->getBroadcastData(currentPosition);
     
-    int bytesWritten = sprintf((char *)currentPosition,
-                               PACKET_FORMAT,
-                               agentData->getPitch(),
-                               agentData->getYaw(),
-                               agentData->getRoll(),
-                               agentData->getHeadPositionX(),
-                               agentData->getHeadPositionY(),
-                               agentData->getHeadPositionZ(),
-                               agentData->getLoudness(),
-                               agentData->getAverageLoudness(),
-                               agentData->getHandPositionX(),
-                               agentData->getHandPositionY(),
-                               agentData->getHandPositionZ());
-
-    currentPosition += bytesWritten;
     return currentPosition;
 }
 
 void attachAvatarDataToAgent(Agent *newAgent) {
     if (newAgent->getLinkedData() == NULL) {
-        newAgent->setLinkedData(new AvatarAgentData());
+        newAgent->setLinkedData(new AvatarData());
     }
 }
 
@@ -78,7 +63,7 @@ int main(int argc, char* argv[])
     agentList->startPingUnknownAgentsThread();
     
     sockaddr *agentAddress = new sockaddr;
-    char *packetData = new char[MAX_PACKET_SIZE];
+    unsigned char *packetData = new unsigned char[MAX_PACKET_SIZE];
     ssize_t receivedBytes = 0;
     
     unsigned char *broadcastPacket = new unsigned char[MAX_PACKET_SIZE];
@@ -92,7 +77,7 @@ int main(int argc, char* argv[])
             switch (packetData[0]) {
                 case PACKET_HEADER_HEAD_DATA:
                     // this is positional data from an agent
-                    agentList->updateAgentWithData(agentAddress, (void *)packetData, receivedBytes);
+                    agentList->updateAgentWithData(agentAddress, packetData, receivedBytes);
                     
                     currentBufferPosition = broadcastPacket + 1;
                     agentIndex = 0;
@@ -116,7 +101,7 @@ int main(int argc, char* argv[])
                     break;
                 default:
                     // hand this off to the AgentList
-                    agentList->processAgentData(agentAddress, (void *)packetData, receivedBytes);
+                    agentList->processAgentData(agentAddress, packetData, receivedBytes);
                     break;
             }
         }

cmake/macros/IncludeGLM.cmake

@@ -1,5 +1,5 @@
-MACRO(INCLUDE_GLM TARGET MACRO_DIR)
+MACRO(INCLUDE_GLM TARGET ROOT_DIR)
-	set(GLM_ROOT_DIR ${MACRO_DIR}/../../externals)
+	set(GLM_ROOT_DIR ${ROOT_DIR}/externals)
 	find_package(GLM REQUIRED)
 	include_directories(${GLM_INCLUDE_DIRS})
-ENDMACRO(INCLUDE_GLM _target _macro_dir)
+ENDMACRO(INCLUDE_GLM _target _root_dir)

cmake/macros/LinkHifiLibrary.cmake

@@ -3,12 +3,6 @@ MACRO(LINK_HIFI_LIBRARY LIBRARY TARGET ROOT_DIR)
         add_subdirectory(${ROOT_DIR}/libraries/${LIBRARY} ${ROOT_DIR}/libraries/${LIBRARY})
     endif (NOT TARGET ${LIBRARY})
     
-    string(TOUPPER ${LIBRARY} UPPERCASED_LIBRARY_NAME)
-    set(HIFI_LIBRARY_PROPERTY "HIFI_${UPPERCASED_LIBRARY_NAME}_LIBRARY")
-    get_directory_property(HIFI_LIBRARY 
-                           DIRECTORY ${ROOT_DIR}/libraries/${LIBRARY} 
-                           DEFINITION ${HIFI_LIBRARY_PROPERTY})
-    
     include_directories(${ROOT_DIR}/libraries/${LIBRARY}/src)
 
     add_dependencies(${TARGET} ${LIBRARY})

cmake/macros/SetupHifiLibrary.cmake

@@ -0,0 +1,9 @@
+MACRO(SETUP_HIFI_LIBRARY TARGET)
+    project(${TARGET_NAME})
+
+    # grab the implemenation and header files
+    file(GLOB LIB_SRCS src/*.h src/*.cpp)
+
+    # create a library and set the property so it can be referenced later
+    add_library(${TARGET_NAME} ${LIB_SRCS})
+ENDMACRO(SETUP_HIFI_LIBRARY _target)

domain-server/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8)
 
-set(ROOT_DIR ../)
+set(ROOT_DIR ..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
 set(TARGET_NAME domain-server)

injector/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8)
 
-set(ROOT_DIR ../)
+set(ROOT_DIR ..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
 set(TARGET_NAME injector)

interface/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8)
 
-set(ROOT_DIR ../)
+set(ROOT_DIR ..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
 set(TARGET_NAME interface)
@@ -26,7 +26,7 @@ endif (WIN32)
 
 # set up the external glm library
 include(${MACRO_DIR}/IncludeGLM.cmake)
-include_glm(${TARGET_NAME} ${MACRO_DIR})
+include_glm(${TARGET_NAME} ${ROOT_DIR})
 
 # create the InterfaceConfig.h file based on GL_HEADERS above
 configure_file(InterfaceConfig.h.in ${PROJECT_BINARY_DIR}/includes/InterfaceConfig.h)
@@ -56,10 +56,11 @@ add_executable(${TARGET_NAME} MACOSX_BUNDLE ${INTERFACE_SRCS})
 
 # link in the hifi shared library
 include(${MACRO_DIR}/LinkHifiLibrary.cmake)
-link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR})
 
-# link in the hifi voxels library
+# link required hifi libraries
+link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR})
 link_hifi_library(voxels ${TARGET_NAME} ${ROOT_DIR})
+link_hifi_library(avatars ${TARGET_NAME} ${ROOT_DIR})
 
 # find required libraries
 find_package(GLM REQUIRED)

interface/src/Audio.cpp

@@ -156,7 +156,7 @@ int audioCallback (const void *inputBuffer,
             
             // memcpy the three float positions
             for (int p = 0; p < 3; p++) {
-                memcpy(currentPacketPtr, &data->linkedHead->getPos()[p], sizeof(float));
+                memcpy(currentPacketPtr, &data->linkedHead->getBodyPosition()[p], sizeof(float));
                 currentPacketPtr += sizeof(float);
             }
             
@@ -411,7 +411,7 @@ void *receiveAudioViaUDP(void *args) {
             }
             if (packetsReceivedThisPlayback == 1) gettimeofday(&firstPlaybackTimer, NULL);
 
-            ringBuffer->parseData(receivedData, PACKET_LENGTH_BYTES);
+            ringBuffer->parseData((unsigned char *)receivedData, PACKET_LENGTH_BYTES);
 
             previousReceiveTime = currentReceiveTime;
         }
@@ -444,8 +444,8 @@ Audio::Audio(Oscilloscope *s, Head *linkedHead)
     // read the walking sound from the raw file and store it
     // in the in memory array
     
-    switchToResourcesIfRequired();
+    switchToResourcesParentIfRequired();
-    FILE *soundFile = fopen("audio/walking.raw", "r");
+    FILE *soundFile = fopen("resources/audio/walking.raw", "r");
     
     // get length of file:
     std::fseek(soundFile, 0, SEEK_END);

interface/src/Camera.cpp

@@ -5,57 +5,68 @@
 //
 //---------------------------------------------------------------------
 
+#include <SharedUtil.h>
+
 #include "Camera.h"
-#include "Util.h"
 
-
+Camera::Camera() {
-
+	_mode			= CAMERA_MODE_THIRD_PERSON;
-//------------------------
+	_tightness		= DEFAULT_CAMERA_TIGHTNESS;
-Camera::Camera()
+	_fieldOfView    = 60.0;     // default
-{
+	_nearClip       = 0.08;     // default
-	mode			= CAMERA_MODE_THIRD_PERSON;
+	_farClip        = 50.0;     // default
-	tightness		= DEFAULT_CAMERA_TIGHTNESS;
+	_yaw            = 0.0;
-	fieldOfView		= 60.0; // default
+	_pitch			= 0.0;
-	yaw				= 0.0;
+	_roll			= 0.0;
-	pitch			= 0.0;
+	_up				= 0.0;
-	roll			= 0.0;
+	_distance		= 0.0;
-	up				= 0.0;
+	_idealYaw		= 0.0;
-	distance		= 0.0;
+	_targetPosition	= glm::vec3( 0.0, 0.0, 0.0 );
-	targetPosition	= glm::vec3( 0.0, 0.0, 0.0 );
+	_position		= glm::vec3( 0.0, 0.0, 0.0 );
-	position		= glm::vec3( 0.0, 0.0, 0.0 );
+	_idealPosition	= glm::vec3( 0.0, 0.0, 0.0 );
-	idealPosition	= glm::vec3( 0.0, 0.0, 0.0 );
+	_orientation.setToIdentity();
-	orientation.setToIdentity();
 }
 
 
 
-
-//------------------------------------
 void Camera::update( float deltaTime ) 
 {
-	double radian = ( yaw / 180.0 ) * PIE;
+	//----------------------------------------
-
+	// derive t from tightness
-	//these need to be checked to make sure they correspond to the cordinate system.
+	//----------------------------------------
-	double x = distance * -sin( radian );
+	float t = _tightness * deltaTime;
-	double z = distance *  cos( radian );
-	double y = up;
-	
-	idealPosition = targetPosition + glm::vec3( x, y, z );
-	
-	float t = tightness * deltaTime;
 	
 	if ( t > 1.0 ){
 		t = 1.0;
 	}
 
-	position += ( idealPosition - position ) * t; 
+	//----------------------------------------
+	// update _yaw (before position!) 
+	//----------------------------------------
+	_yaw += ( _idealYaw - _yaw ) * t;
+	float radian = ( _yaw / 180.0 ) * PIE;
 
-	//-------------------------------------------------------------------------
+	//----------------------------------------
-	//geterate the ortho-normals for the orientation based on the Euler angles
+	// update _position
-	//-------------------------------------------------------------------------
+	//----------------------------------------
-	orientation.setToIdentity();
+	//these need to be checked to make sure they correspond to the coordinate system.
-	orientation.yaw		( yaw	);
+	double x = _distance * -sin( radian );
-	orientation.pitch	( pitch	);
+	double z = _distance *  cos( radian );
-	orientation.roll	( roll	);
+	double y = _up;
+		
+	_idealPosition = _targetPosition + glm::vec3( x, y, z );
+	
+	_position += ( _idealPosition - _position ) * t; 
+
+	//------------------------------------------------------------------------------
+	// generate the ortho-normals for the orientation based on the Euler angles
+	//------------------------------------------------------------------------------
+	_orientation.setToIdentity();
+    
+	_orientation.yaw	( _yaw	 );
+	_orientation.pitch	( _pitch );
+	_orientation.roll	( _roll	 );
+    
+    //printf( "orientation.front = %f, %f, %f\n", _orientation.front.x, _orientation.front.y, _orientation.front.z );
 }
 

interface/src/Camera.h

@@ -29,38 +29,50 @@ public:
 
 	void update( float deltaTime );
 	
-	void setMode			( CameraMode	m ) { mode				= m; }
+    void setMode            ( CameraMode    m ) { _mode             = m; }
-	void setYaw				( float			y ) { yaw				= y; }
+    void setYaw             ( float         y ) { _idealYaw         = y; }
-	void setPitch			( float			p ) { pitch				= p; }
+    void setPitch           ( float         p ) { _pitch            = p; }
-	void setRoll			( float			r ) { roll				= r; }
+    void setRoll            ( float         r ) { _roll             = r; }
-	void setUp				( float			u ) { up				= u; }
+    void setUp              ( float         u ) { _up               = u; }
-	void setDistance		( float			d ) { distance			= d; }
+    void setDistance        ( float         d ) { _distance         = d; }
-	void setTargetPosition	( glm::vec3		t ) { targetPosition	= t; }
+    void setTargetPosition  ( glm::vec3     t ) { _targetPosition   = t; };
-	void setPosition		( glm::vec3		p ) { position			= p; }
+    void setPosition        ( glm::vec3     p ) { _position         = p; };
-	void setTightness		( float			t ) { tightness			= t; }
+    void setOrientation     ( Orientation   o ) { _orientation.set(o);   }
-	void setOrientation		( Orientation	o ) { orientation.set(o); }
+    void setTightness       ( float         t ) { _tightness        = t; }
+    void setFieldOfView     ( float         f ) { _fieldOfView      = f; }
+    void setAspectRatio     ( float         a ) { _aspectRatio      = a; }
+    void setNearClip        ( float         n ) { _nearClip         = n; }
+    void setFarClip         ( float         f ) { _farClip          = f; }
 
-	float		getYaw			() { return yaw;			}
+    float       getYaw              () { return _yaw;               }
-	float		getPitch		() { return pitch;			}
+    float       getPitch            () { return _pitch;             }
-	float		getRoll			() { return roll;			}
+    float       getRoll             () { return _roll;              }
-	glm::vec3	getPosition		() { return position;		}
+    glm::vec3   getPosition         () { return _position;          }
-	Orientation	getOrientation	() { return orientation;	}
+    Orientation getOrientation      () { return _orientation;       }
-	CameraMode	getMode			() { return mode;			}
+    CameraMode  getMode             () { return _mode;              }
+    float       getFieldOfView      () { return _fieldOfView;       }
+    float       getAspectRatio      () { return _aspectRatio;       }
+    float       getNearClip         () { return _nearClip;          }
+    float       getFarClip          () { return _farClip;           }
 
 private:
 
-	CameraMode	mode;
+	CameraMode	_mode;
-	glm::vec3	position;
+	glm::vec3	_position;
-	glm::vec3	idealPosition;
+	glm::vec3	_idealPosition;
-	glm::vec3	targetPosition;
+	glm::vec3	_targetPosition;
-	float		fieldOfView;
+	float		_fieldOfView;
-	float		yaw;
+    float		_aspectRatio;
-	float		pitch;
+    float		_nearClip;
-	float		roll;
+    float		_farClip;
-	float		up;
+	float		_yaw;
-	float		distance;
+	float		_pitch;
-	float		tightness;
+	float		_roll;
-	Orientation	orientation;
+	float		_up;
+	float		_idealYaw;
+	float		_distance;
+	float		_tightness;
+	Orientation	_orientation;
 };
 
 #endif

interface/src/Head.cpp

@@ -17,6 +17,7 @@
 #include "Head.h"
 #include <AgentList.h>
 #include <AgentTypes.h>
+#include <PacketHeaders.h>
 
 using namespace std;
 
@@ -44,12 +45,29 @@ vector<unsigned char> iris_texture;
 unsigned int iris_texture_width = 512;
 unsigned int iris_texture_height = 256;
 
-
-
-
 Head::Head() {
 	initializeAvatar();
     
+    
+	avatar.velocity		= glm::vec3( 0.0, 0.0, 0.0 );
+	avatar.thrust		= glm::vec3( 0.0, 0.0, 0.0 );
+	avatar.orientation.setToIdentity();
+	
+	closestOtherAvatar = 0;
+	
+	_bodyYaw		= -90.0;
+	_bodyPitch	= 0.0;
+	_bodyRoll	= 0.0;
+	
+	bodyYawDelta = 0.0;
+	
+	triggeringAction = false;
+	
+	mode = AVATAR_MODE_STANDING;
+
+    initializeSkeleton();
+    
+    
     for (int i = 0; i < MAX_DRIVE_KEYS; i++) driveKeys[i] = false; 
     
     PupilSize = 0.10;
@@ -96,7 +114,7 @@ Head::Head() {
 	springVelocityDecay		= 16.0f;
     
     if (iris_texture.size() == 0) {
-        switchToResourcesIfRequired();
+        switchToResourcesParentIfRequired();
         unsigned error = lodepng::decode(iris_texture, iris_texture_width, iris_texture_height, iris_texture_file);
         if (error != 0) {
             std::cout << "error " << error << ": " << lodepng_error_text(error) << std::endl;
@@ -124,7 +142,24 @@ Head::Head() {
 Head::Head(const Head &otherHead) {
 	initializeAvatar();
     
-    bodyPosition = otherHead.bodyPosition;
+	avatar.velocity		= otherHead.avatar.velocity;
+	avatar.thrust		= otherHead.avatar.thrust;
+	avatar.orientation.set( otherHead.avatar.orientation );
+	
+	closestOtherAvatar = otherHead.closestOtherAvatar;
+	
+	_bodyYaw	= otherHead._bodyYaw;
+	_bodyPitch	= otherHead._bodyPitch;
+	_bodyRoll	= otherHead._bodyRoll;
+	
+	bodyYawDelta = otherHead.bodyYawDelta;
+	
+	triggeringAction = otherHead.triggeringAction;
+	
+	mode = otherHead.mode;
+
+    initializeSkeleton();
+    
 
     for (int i = 0; i < MAX_DRIVE_KEYS; i++) driveKeys[i] = otherHead.driveKeys[i];
 
@@ -218,6 +253,7 @@ void Head::UpdatePos(float frametime, SerialInterface * serialInterface, int hea
 
     if ((Pitch < MAX_PITCH) && (Pitch > MIN_PITCH))
         addPitch(measured_pitch_rate * -HEAD_ROTATION_SCALE * frametime);
+    
     addRoll(-measured_roll_rate * HEAD_ROLL_SCALE * frametime);
 
     if (head_mirror) {
@@ -301,14 +337,14 @@ void Head::simulate(float deltaTime) {
 	//------------------------
 	// update avatar skeleton
 	//------------------------ 
-	updateAvatarSkeleton();
+	updateSkeleton();
 	
 	//------------------------------------------------------------------------
 	// reset hand and elbow position according to hand movement
 	//------------------------------------------------------------------------
 	if ( handBeingMoved ){
 		if (! previousHandBeingMoved ){ 
-			initializeAvatarSprings();
+			initializeBodySprings();
 			usingSprings = true;
 			//printf( "just started moving hand\n" );
 		}
@@ -322,7 +358,7 @@ void Head::simulate(float deltaTime) {
 	
 	if ( handBeingMoved ) {
 		updateHandMovement();
-		updateAvatarSprings( deltaTime );
+		updateBodySprings( deltaTime );
 	}
 
 	previousHandBeingMoved = handBeingMoved;
@@ -379,12 +415,12 @@ void Head::simulate(float deltaTime) {
 	//----------------------------------------------------------
 	// update body yaw by body yaw delta
 	//----------------------------------------------------------
-	bodyYaw += bodyYawDelta * deltaTime;
+	_bodyYaw += bodyYawDelta * deltaTime;
 	
 	//----------------------------------------------------------
 	// (for now) set head yaw to body yaw
 	//----------------------------------------------------------
-	Yaw = bodyYaw;
+	Yaw = _bodyYaw;
 	
 	//----------------------------------------------------------
 	// decay body yaw delta
@@ -400,7 +436,7 @@ void Head::simulate(float deltaTime) {
 	//----------------------------------------------------------
 	// update position by velocity
 	//----------------------------------------------------------
-	bodyPosition += (glm::vec3)avatar.velocity * deltaTime;
+	_bodyPosition += (glm::vec3)avatar.velocity * deltaTime;
 
 	//----------------------------------------------------------
 	// decay velocity
@@ -511,11 +547,12 @@ void Head::simulate(float deltaTime) {
 	  
 	   
 void Head::render(int faceToFace, int isMine) {
+
 	//---------------------------------------------------
 	// show avatar position
 	//---------------------------------------------------
 	glPushMatrix();
-		glTranslatef( bodyPosition.x, bodyPosition.y, bodyPosition.z );
+		glTranslatef(_bodyPosition.x, _bodyPosition.y, _bodyPosition.z);
 		glScalef( 0.03, 0.03, 0.03 );
         glutSolidSphere( 1, 10, 10 );
 	glPopMatrix();
@@ -564,6 +601,8 @@ void Head::render(int faceToFace, int isMine) {
 
 
 
+//this has been moved to Utils.cpp
+/*
 void Head::renderOrientationDirections( glm::vec3 position, Orientation orientation, float size ) {
 	glm::vec3 pRight	= position + orientation.right	* size;
 	glm::vec3 pUp		= position + orientation.up		* size;
@@ -571,22 +610,23 @@ void Head::renderOrientationDirections( glm::vec3 position, Orientation orientat
 		
 	glColor3f( 1.0f, 0.0f, 0.0f );
 	glBegin( GL_LINE_STRIP );
-	glVertex3f( bone[ AVATAR_BONE_HEAD ].position.x, bone[ AVATAR_BONE_HEAD ].position.y, bone[ AVATAR_BONE_HEAD ].position.z );
+	glVertex3f( position.x, position.y, position.z );
 	glVertex3f( pRight.x, pRight.y, pRight.z );
 	glEnd();
 
 	glColor3f( 0.0f, 1.0f, 0.0f );
 	glBegin( GL_LINE_STRIP );
-	glVertex3f( bone[ AVATAR_BONE_HEAD ].position.x, bone[ AVATAR_BONE_HEAD ].position.y, bone[ AVATAR_BONE_HEAD ].position.z );
+	glVertex3f( position.x, position.y, position.z );
 	glVertex3f( pUp.x, pUp.y, pUp.z );
 	glEnd();
 
 	glColor3f( 0.0f, 0.0f, 1.0f );
 	glBegin( GL_LINE_STRIP );
-	glVertex3f( bone[ AVATAR_BONE_HEAD ].position.x, bone[ AVATAR_BONE_HEAD ].position.y, bone[ AVATAR_BONE_HEAD ].position.z );
+	glVertex3f( position.x, position.y, position.z );
 	glVertex3f( pFront.x, pFront.y, pFront.z );
 	glEnd();
 }
+*/
 
 	  
 	   
@@ -619,7 +659,7 @@ void Head::renderHead( int faceToFace, int isMine ) {
 	
 	glScalef( 0.03, 0.03, 0.03 );
 
-    glRotatef( bodyYaw, 0, 1, 0);
+    glRotatef(_bodyYaw, 0, 1, 0);
     
 		
 	//  Don't render a head if it is really close to your location, because that is your own head!
@@ -771,15 +811,16 @@ AvatarMode Head::getMode() {
 
 
 void Head::initializeAvatar() {
+/*
 	avatar.velocity		= glm::vec3( 0.0, 0.0, 0.0 );
 	avatar.thrust		= glm::vec3( 0.0, 0.0, 0.0 );
 	avatar.orientation.setToIdentity();
 	
 	closestOtherAvatar = 0;
 	
-	bodyYaw		= -90.0;
+	_bodyYaw		= -90.0;
-	bodyPitch	= 0.0;
+	_bodyPitch	= 0.0;
-	bodyRoll	= 0.0;
+	_bodyRoll	= 0.0;
 	
 	bodyYawDelta = 0.0;
 	
@@ -787,6 +828,15 @@ void Head::initializeAvatar() {
 	
 	mode = AVATAR_MODE_STANDING;
 
+    initializeSkeleton();
+    */
+}
+
+
+
+
+void Head::initializeSkeleton() {
+
 	for (int b=0; b<NUM_AVATAR_BONES; b++) {
 		bone[b].position			= glm::vec3( 0.0, 0.0, 0.0 );
 		bone[b].springyPosition	= glm::vec3( 0.0, 0.0, 0.0 );
@@ -878,10 +928,12 @@ void Head::initializeAvatar() {
 	//----------------------------------------------------------------------------
 	// generate world positions
 	//----------------------------------------------------------------------------
-	updateAvatarSkeleton();
+	updateSkeleton();
 }
 
 
+
+
 void Head::calculateBoneLengths() {
 	for (int b=0; b<NUM_AVATAR_BONES; b++) {
 		bone[b].length = glm::length( bone[b].defaultPosePosition );
@@ -895,29 +947,25 @@ void Head::calculateBoneLengths() {
 
 
 
-void Head::updateAvatarSkeleton() {
+void Head::updateSkeleton() {
 	//----------------------------------
 	// rotate body...
 	//----------------------------------	
 	avatar.orientation.setToIdentity();
-	avatar.orientation.yaw( bodyYaw );
+	avatar.orientation.yaw( _bodyYaw );
 
 	//------------------------------------------------------------------------
 	// calculate positions of all bones by traversing the skeleton tree:
 	//------------------------------------------------------------------------
 	for (int b=0; b<NUM_AVATAR_BONES; b++) {	
 		if ( bone[b].parent == AVATAR_BONE_NULL ) {
-			bone[b].orientation.set( avatar.orientation );
+			bone[b].orientation.set(avatar.orientation);
-			
+			//printf( "bodyPosition = %f, %f, %f\n", bodyPosition.x, bodyPosition.y, bodyPosition.z );
-//printf( "bodyPosition = %f, %f, %f\n", bodyPosition.x, bodyPosition.y, bodyPosition.z );			
+			glm::vec3 ppp = _bodyPosition;
-
-			glm::vec3 ppp = bodyPosition;
 			
 //			ppp.y += 0.2;
 			
 			bone[b].position = ppp;// + glm::vec3( 0.0f, 1.0f, 0.0f ) * 1.0f;
-					
-			
 		}
 		else {
 			bone[b].orientation.set( bone[ bone[b].parent ].orientation );
@@ -934,7 +982,7 @@ void Head::updateAvatarSkeleton() {
 }
 
 
-void Head::initializeAvatarSprings() {
+void Head::initializeBodySprings() {
 	for (int b=0; b<NUM_AVATAR_BONES; b++) {
 		bone[b].springyPosition = bone[b].position;
 		bone[b].springyVelocity = glm::vec3( 0.0f, 0.0f, 0.0f );
@@ -942,12 +990,12 @@ void Head::initializeAvatarSprings() {
 }
 
 
-void Head::updateAvatarSprings( float deltaTime ) {
+void Head::updateBodySprings( float deltaTime ) {
 	for (int b=0; b<NUM_AVATAR_BONES; b++) {
 		glm::vec3 springVector( bone[b].springyPosition );
 
 		if ( bone[b].parent == AVATAR_BONE_NULL ) {
-			springVector -= bodyPosition;
+			springVector -= _bodyPosition;
 		}
 		else {
 			springVector -= bone[ bone[b].parent ].springyPosition;
@@ -980,7 +1028,7 @@ void Head::updateAvatarSprings( float deltaTime ) {
 }
 
 float Head::getBodyYaw() {
-	return bodyYaw;
+	return _bodyYaw;
 }
 
 glm::vec3 Head::getHeadLookatDirection() {
@@ -1019,17 +1067,6 @@ glm::vec3 Head::getHeadPosition() {
 	);
 }
 
-
-glm::vec3 Head::getBodyPosition() {
-	return glm::vec3
-	(
-		bone[ AVATAR_BONE_PELVIS_SPINE ].position.x,
-		bone[ AVATAR_BONE_PELVIS_SPINE ].position.y,
-		bone[ AVATAR_BONE_PELVIS_SPINE ].position.z
-	);
-}
-
-
 void Head::updateHandMovement() {
 	glm::vec3 transformedHandMovement;
 	
@@ -1188,48 +1225,12 @@ void Head::renderBody() {
 	
 }
 
-
-
-// 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
-    sprintf(data, "H%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
-            getRenderPitch() + Pitch, -getRenderYaw() + 180 -Yaw, Roll,
-			//avatar.yaw, avatar.pitch, avatar.roll,
-            bodyPosition.x + leanSideways, bodyPosition.y, bodyPosition.z + leanForward,
-            loudness, averageLoudness,
-            //hand->getPos().x, hand->getPos().y, hand->getPos().z);  //previous to Ventrella change
-            bone[ AVATAR_BONE_RIGHT_HAND ].position.x, 
-			bone[ AVATAR_BONE_RIGHT_HAND ].position.y, 
-			bone[ AVATAR_BONE_RIGHT_HAND ].position.z ); 
-    return strlen(data);
-}
-
-
-//called on the other agents - assigns it to my views of the others
-void Head::parseData(void *data, int size) {
-	sscanf
-	(
-		(char *)data, "H%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
-		&Pitch, &Yaw, &Roll,
-		//&avatar.yaw, &avatar.pitch, &avatar.roll,
-		&bodyPosition.x, &bodyPosition.y, &bodyPosition.z,
-		&loudness, &averageLoudness,
-		&bone[ AVATAR_BONE_RIGHT_HAND ].position.x, 
-		&bone[ AVATAR_BONE_RIGHT_HAND ].position.y, 
-		&bone[ AVATAR_BONE_RIGHT_HAND ].position.z
-	);
-	
-	handBeingMoved = true;
-}
-
 void Head::SetNewHeadTarget(float pitch, float yaw) {
     PitchTarget = pitch;
     YawTarget = yaw;
 }
 
-void Head::processTransmitterData(char *packetData, int numBytes) {
+void Head::processTransmitterData(unsigned char* packetData, int numBytes) {
     //  Read a packet from a transmitter app, process the data
     float accX, accY, accZ,
     graX, graY, graZ,

interface/src/Head.h

@@ -10,10 +10,13 @@
 #define __interface__head__
 
 #include <iostream>
-#include "AgentData.h"
+
+#include <AvatarData.h>
+#include <Orientation.h>
+
 #include "Field.h"
 #include "world.h"
-#include "Orientation.h"	// added by Ventrella as a utility
+
 #include "InterfaceConfig.h"
 #include "SerialInterface.h"
 
@@ -39,48 +42,6 @@ enum AvatarMode
 	NUM_AVATAR_MODES
 };
 
-
-
-/*
-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,
@@ -134,7 +95,7 @@ struct Avatar
 	Orientation	orientation;
 };
 
-class Head : public AgentData {
+class Head : public AvatarData {
     public:
         Head();
         ~Head();
@@ -169,8 +130,6 @@ class Head : public AgentData {
 		glm::vec3 getHeadLookatDirectionRight();
 		glm::vec3 getHeadPosition();
 		glm::vec3 getBonePosition( AvatarBones b );		
-		glm::vec3 getBodyPosition();
-		
 		
 		AvatarMode getMode();
 		
@@ -180,25 +139,19 @@ class Head : public AgentData {
 		
 		void renderBody();
 		void renderHead( int faceToFace, int isMine );
-		void renderOrientationDirections( glm::vec3 position, Orientation orientation, float size );
+		//void renderOrientationDirections( glm::vec3 position, Orientation orientation, float size );
 
         void simulate(float);
 				
 		void setHandMovement( glm::vec3 movement );
 		void updateHandMovement();
         
-        //  Send and receive network data
-        int getBroadcastData(char * data);
-        void parseData(void *data, int size);
-        
         float getLoudness() {return loudness;};
         float getAverageLoudness() {return averageLoudness;};
         void setAverageLoudness(float al) {averageLoudness = al;};
         void setLoudness(float l) {loudness = l;};
         
         void SetNewHeadTarget(float, float);
-        glm::vec3 getPos() { return bodyPosition; };
-        void setPos(glm::vec3 newpos) { bodyPosition = newpos; };
     
         //  Set what driving keys are being pressed to control thrust levels
         void setDriveKeys(int key, bool val) { driveKeys[key] = val; };
@@ -213,7 +166,7 @@ class Head : public AgentData {
         //  Related to getting transmitter UDP data used to animate the avatar hand
         //
 
-        void processTransmitterData(char * packetData, int numBytes);
+        void processTransmitterData(unsigned char * packetData, int numBytes);
         float getTransmitterHz() { return transmitterHz; };
     
     private:
@@ -251,17 +204,14 @@ class Head : public AgentData {
         float audioAttack;
         float browAudioLift;
 		
-        glm::vec3 bodyPosition;
-		
 		bool triggeringAction;
     
-		float bodyYaw;
-		float bodyPitch;
-		float bodyRoll;
 		float bodyYawDelta;
 		
 		float		closeEnoughToInteract;
 		int			closestOtherAvatar;
+        
+        //temporary - placeholder for real other avs
 		glm::vec3	DEBUG_otherAvatarListPosition	[ NUM_OTHER_AVATARS ];
 		float		DEBUG_otherAvatarListTimer		[ NUM_OTHER_AVATARS ];
 		
@@ -286,13 +236,6 @@ class Head : public AgentData {
 		
 		AvatarMode mode;
 
-		void initializeAvatar();
-		void updateAvatarSkeleton();
-		void initializeAvatarSprings();
-		void updateAvatarSprings( float deltaTime );
-		void calculateBoneLengths();
-    
-        void readSensors();
         float renderYaw, renderPitch;       //   Pitch from view frustum when this is own head.
     
         //
@@ -302,6 +245,18 @@ class Head : public AgentData {
         float transmitterHz;
         int transmitterPackets;
 
+        
+        //-------------------------------------------
+        // private methods...
+        //-------------------------------------------
+		void initializeAvatar();
+		void initializeSkeleton();
+		void updateSkeleton();
+		void initializeBodySprings();
+		void updateBodySprings( float deltaTime );
+		void calculateBoneLengths();
+    
+        void readSensors();
 };
 
 #endif

interface/src/Orientation.cpp

@@ -1,82 +0,0 @@
-//-----------------------------------------------------------
-//
-// Created by Jeffrey Ventrella  
-// Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
-//
-//-----------------------------------------------------------
-
-#include "Orientation.h"
-#include "Util.h"
-
-
-Orientation::Orientation() {
-	right	= glm::vec3(  1.0,  0.0,  0.0 );
-	up		= glm::vec3(  0.0,  1.0,  0.0 );
-	front	= glm::vec3(  0.0,  0.0,  1.0 );	
-}
-
-
-void Orientation::setToIdentity() {
-	right	= glm::vec3(  1.0,  0.0,  0.0 );
-	up		= glm::vec3(  0.0,  1.0,  0.0 );
-	front	= glm::vec3(  0.0,  0.0,  1.0 );	
-}
-
-
-void Orientation::set( Orientation o ) { 
-	right	= o.right;
-	up		= o.up;
-	front	= o.front;	
-}
-
-
-void Orientation::yaw( float angle ) {
-	float r = angle * PI_OVER_180;
-	float s = sin(r);
-	float c = cos(r);
-	
-	glm::vec3 cosineFront	= front * c;
-	glm::vec3 cosineRight	= right * c;
-	glm::vec3 sineFront		= front * s;
-	glm::vec3 sineRight		= right * s;
-	
-	front	= cosineFront + sineRight;
-	right	= cosineRight - sineFront;	
-}
-
-
-void Orientation::pitch( float angle ) {
-	float r = angle * PI_OVER_180;
-	float s = sin(r);
-	float c = cos(r);
-	
-	glm::vec3 cosineUp		= up	* c;
-	glm::vec3 cosineFront	= front	* c;
-	glm::vec3 sineUp		= up	* s;
-	glm::vec3 sineFront		= front * s;
-	
-	up		= cosineUp		+ sineFront;
-	front	= cosineFront	- sineUp;
-}
-
-
-void Orientation::roll( float angle ) {
-	float r = angle * PI_OVER_180;
-	float s = sin(r);
-	float c = cos(r);
-	
-	glm::vec3 cosineUp		= up	* c;
-	glm::vec3 cosineRight	= right	* c;
-	glm::vec3 sineUp		= up	* s;
-	glm::vec3 sineRight		= right	* s;
-	
-	up		= cosineUp		+ sineRight;
-	right	= cosineRight	- sineUp;	
-}
-
-
-void Orientation::setRightUpFront( const glm::vec3 &r, const glm::vec3 &u, const glm::vec3 &f ) {
-	right	= r;
-	up		= u;
-	front	= f;
-}

interface/src/SerialInterface.cpp

@@ -6,12 +6,12 @@
 //
 //  Channels are received in the following order (integer 0-4096 based on voltage 0-3.3v)
 //
-//  AIN 15:  Pitch Gyro (nodding your head 'yes')
+//  0 - AIN 15:  Pitch Gyro (nodding your head 'yes')
-//  AIN 16:  Yaw Gyro (shaking your head 'no')
+//  1 - AIN 16:  Yaw Gyro (shaking your head 'no')
-//  AIN 17:  Roll Gyro (looking quizzical, tilting your head)
+//  2 - AIN 17:  Roll Gyro (looking quizzical, tilting your head)
-//  AIN 18:  Lateral acceleration (moving from side-to-side in front of your monitor)
+//  3 - AIN 18:  Lateral acceleration (moving from side-to-side in front of your monitor)
-//  AIN 19:  Up/Down acceleration (sitting up/ducking in front of your monitor)
+//  4 - AIN 19:  Up/Down acceleration (sitting up/ducking in front of your monitor)
-//  AIN 20:  Forward/Back acceleration (Toward or away from your monitor)
+//  5 - AIN 20:  Forward/Back acceleration (Toward or away from your monitor)
 //
 
 #include "SerialInterface.h"
@@ -21,14 +21,15 @@
 #include <sys/time.h>
 #endif
 
-int serial_fd;
+int serialFd;
 const int MAX_BUFFER = 255;
-char serial_buffer[MAX_BUFFER];
+char serialBuffer[MAX_BUFFER];
-int serial_buffer_pos = 0;
+int serialBufferPos = 0;
 
 const int ZERO_OFFSET = 2048;
 const short NO_READ_MAXIMUM_MSECS = 3000;
-const short SAMPLES_TO_DISCARD = 100;
+const short SAMPLES_TO_DISCARD = 100;               //  Throw out the first few samples
+const int GRAVITY_SAMPLES = 200;                    //  Use the first samples to compute gravity vector
 
 void SerialInterface::pair() {
     
@@ -48,7 +49,7 @@ void SerialInterface::pair() {
                 char *serialPortname = new char[100];
                 sprintf(serialPortname, "/dev/%s", entry->d_name);
                 
-                init(serialPortname, 115200);
+                initializePort(serialPortname, 115200);
                 
                 delete [] serialPortname;
             }
@@ -60,18 +61,20 @@ void SerialInterface::pair() {
     
 }
 
-//  Init the serial port to the specified values
+//  Connect to the serial port
-int SerialInterface::init(char* portname, int baud)
+int SerialInterface::initializePort(char* portname, int baud)
 {
 #ifdef __APPLE__
-    serial_fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
+    serialFd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
     
-    printf("Attemping to open serial interface: %s\n", portname);
+    printf("Opening SerialUSB %s: ", portname);
-    
-    if (serial_fd == -1) return -1;     //  Failed to open port
     
+    if (serialFd == -1) {
+        printf("Failed.\n");
+        return -1;     //  Failed to open port
+    }    
     struct termios options;
-    tcgetattr(serial_fd,&options);
+    tcgetattr(serialFd,&options);
     switch(baud)
     {
 		case 9600: cfsetispeed(&options,B9600);
@@ -95,10 +98,10 @@ int SerialInterface::init(char* portname, int baud)
     options.c_cflag &= ~CSTOPB;
     options.c_cflag &= ~CSIZE;
     options.c_cflag |= CS8;
-    tcsetattr(serial_fd,TCSANOW,&options);
+    tcsetattr(serialFd,TCSANOW,&options);
 
     
-    printf("Serial interface opened!\n");
+    printf("Connected.\n");
     resetSerial();    
     active = true;
  #endif
@@ -126,22 +129,17 @@ void SerialInterface::renderLevels(int width, int height) {
         glBegin(GL_LINES);
         glVertex2f(disp_x, height*0.95);
         glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096));
+        glColor4f(1, 0, 0, 1);
+        glVertex2f(disp_x - 3, height*(0.25 + 0.75f*getValue(i)/4096));
+        glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096));
         glEnd();
         //  Trailing Average value
-        glColor4f(1, 1, 0, 1);
         glBegin(GL_LINES);
-        glVertex2f(disp_x + 2, height*0.95);
+        glColor4f(1, 1, 1, 1);
-        glVertex2f(disp_x + 2, height*(0.25 + 0.75f*getTrailingValue(i)/4096));
+        glVertex2f(disp_x, height*(0.25 + 0.75f*getTrailingValue(i)/4096));
+        glVertex2f(disp_x + 4, height*(0.25 + 0.75f*getTrailingValue(i)/4096));
         glEnd();
         
-        /*
-        glColor3f(1,0,0);
-        glBegin(GL_LINES);
-        glLineWidth(4.0);
-        glVertex2f(disp_x - 10, height*0.5 - getValue(i)/4096);
-        glVertex2f(disp_x + 10, height*0.5 - getValue(i)/4096);
-        glEnd();
-        */
         sprintf(val, "%d", getValue(i));
         drawtext(disp_x-GAP/2, (height*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0);
         
@@ -162,20 +160,20 @@ void SerialInterface::renderLevels(int width, int height) {
 }
 void SerialInterface::readData() {
 #ifdef __APPLE__
-    //  This array sets the rate of trailing averaging for each channel.
+    //  This array sets the rate of trailing averaging for each channel:
     //  If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average
-    const float AVG_RATE[] =  {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
+    const float AVG_RATE[] =  {0.002, 0.002, 0.002, 0.002, 0.002, 0.002};
     char bufchar[1];
     
     int initialSamples = totalSamples;
     
-    while (read(serial_fd, &bufchar, 1) > 0) {        
+    while (read(serialFd, &bufchar, 1) > 0) {        
         //std::cout << bufchar[0];
-        serial_buffer[serial_buffer_pos] = bufchar[0];
+        serialBuffer[serialBufferPos] = bufchar[0];
-        serial_buffer_pos++;
+        serialBufferPos++;
         //  Have we reached end of a line of input?
-        if ((bufchar[0] == '\n') || (serial_buffer_pos >= MAX_BUFFER)) {
+        if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) {
-            std::string serialLine(serial_buffer, serial_buffer_pos-1);
+            std::string serialLine(serialBuffer, serialBufferPos-1);
             //std::cout << serialLine << "\n";
             int spot;
             //int channel = 0;
@@ -191,6 +189,7 @@ void SerialInterface::readData() {
                 serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1);
             }
             
+            //  Update Trailing Averages
             for (int i = 0; i < NUM_CHANNELS; i++) {
                 if (totalSamples > SAMPLES_TO_DISCARD) {
                     trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] +
@@ -200,8 +199,21 @@ void SerialInterface::readData() {
                 }
                    
             }
+            
+            //  Use a set of initial samples to compute gravity 
+            if (totalSamples < GRAVITY_SAMPLES) {
+                gravity.x += lastMeasured[ACCEL_X];
+                gravity.y += lastMeasured[ACCEL_Y];
+                gravity.z += lastMeasured[ACCEL_Z];
+            }
+            if (totalSamples == GRAVITY_SAMPLES) {
+                gravity = glm::normalize(gravity);
+                std::cout << "gravity: " << gravity.x << "," <<
+                            gravity.y << "," << gravity.z << "\n";
+            }
+
             totalSamples++;
-            serial_buffer_pos = 0;
+            serialBufferPos = 0;
         }
     }
     
@@ -210,7 +222,7 @@ void SerialInterface::readData() {
         gettimeofday(&now, NULL);
         
         if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
-            std::cout << "No data coming over serial. Shutting down SerialInterface.\n";
+            std::cout << "No data - Shutting down SerialInterface.\n";
             resetSerial();
         }
     } else {
@@ -223,6 +235,7 @@ void SerialInterface::resetSerial() {
 #ifdef __APPLE__
     active = false;
     totalSamples = 0;
+    gravity = glm::vec3(0,-1,0);
     
     gettimeofday(&lastGoodRead, NULL);
     
@@ -233,7 +246,7 @@ void SerialInterface::resetSerial() {
     }
     //  Clear serial input buffer
     for (int i = 1; i < MAX_BUFFER; i++) {
-        serial_buffer[i] = ' ';
+        serialBuffer[i] = ' ';
     }
 #endif
 }

interface/src/SerialInterface.h

@@ -45,8 +45,10 @@ public:
     void resetTrailingAverages();
     void renderLevels(int width, int height);
     bool active;
+    glm::vec3 getGravity() {return gravity;};
+    
 private:
-    int init(char * portname, int baud);
+    int initializePort(char * portname, int baud);
     void resetSerial();
     int lastMeasured[NUM_CHANNELS];
     float trailingAverage[NUM_CHANNELS];
@@ -54,6 +56,7 @@ private:
     int LED;
     int totalSamples;
     timeval lastGoodRead;
+    glm::vec3 gravity;
 };
 
 #endif

interface/src/Util.cpp

@@ -8,11 +8,15 @@
 
 #include "InterfaceConfig.h"
 #include <iostream>
-#include <glm/glm.hpp>
 #include <cstring>
+
+#include <glm/glm.hpp>
+#include <SharedUtil.h>
+
 #include "world.h"
 #include "Util.h"
 
+
 //  Return the azimuth angle in degrees between two points.
 float azimuth_to(glm::vec3 head_pos, glm::vec3 source_pos) {
     return atan2(head_pos.x - source_pos.x, head_pos.z - source_pos.z) * 180.0f / PIf;
@@ -199,3 +203,111 @@ void drawGroundPlaneGrid( float size, int resolution )
 }
 
 
+void renderOrientationDirections( glm::vec3 position, Orientation orientation, float size ) {
+	glm::vec3 pRight	= position + orientation.right	* size;
+	glm::vec3 pUp		= position + orientation.up		* size;
+	glm::vec3 pFront	= position + orientation.front	* size;
+		
+	glColor3f( 1.0f, 0.0f, 0.0f );
+	glBegin( GL_LINE_STRIP );
+	glVertex3f( position.x, position.y, position.z );
+	glVertex3f( pRight.x, pRight.y, pRight.z );
+	glEnd();
+
+	glColor3f( 0.0f, 1.0f, 0.0f );
+	glBegin( GL_LINE_STRIP );
+	glVertex3f( position.x, position.y, position.z );
+	glVertex3f( pUp.x, pUp.y, pUp.z );
+	glEnd();
+
+	glColor3f( 0.0f, 0.0f, 1.0f );
+	glBegin( GL_LINE_STRIP );
+	glVertex3f( position.x, position.y, position.z );
+	glVertex3f( pFront.x, pFront.y, pFront.z );
+	glEnd();
+}
+
+void testOrientationClass() {
+    printf("\n----------\ntestOrientationClass()\n----------\n\n");
+    
+    oTestCase tests[] = { 
+        //       - inputs ------------, outputs --------------------  -------------------  ----------------------------
+        //                              -- front -------------------, -- up -------------, -- right -------------------
+        //       ( yaw  , pitch, roll , front.x , front.y , front.z , up.x , up.y , up.z , right.x , right.y , right.z  )
+
+        // simple yaw tests
+        oTestCase( 0.f  , 0.f  , 0.f  ,  0.f    , 0.f     , 1.0f    ,  0.f , 1.0f , 0.f  , -1.0f   , 0.f     , 0.f      ),
+        oTestCase( 90.0f, 0.f  , 0.f  , 1.0f    , 0.f     , 0.f     ,  0.f , 1.0f , 0.f  , 0.0f    , 0.f     , 1.0f     ),
+        oTestCase(180.0f, 0.f  , 0.f  ,  0.f    , 0.f     , -1.0f   ,  0.f , 1.0f , 0.f  , 1.0f    , 0.f     , 0.f      ),
+        oTestCase(270.0f, 0.f  , 0.f  , -1.0f   , 0.f     , 0.f     ,  0.f , 1.0f , 0.f  , 0.0f    , 0.f     , -1.0f    ),
+
+        // simple pitch tests
+        oTestCase( 0.f  ,90.f  , 0.f  ,  0.f    , 1.0f     , 0.0f   ,  0.f , 0.0f , -1.0f, -1.0f   , 0.f     , 0.f      ),
+
+    };
+    
+    int failedCount = 0;
+    int totalTests = sizeof(tests)/sizeof(oTestCase);
+    
+    for (int i=0; i < totalTests; i++) {
+    
+        bool passed = true; // I'm an optimist!
+
+        float yaw   = tests[i].yaw;
+        float pitch = tests[i].pitch;
+        float roll  = tests[i].roll;
+
+        Orientation o1;
+        o1.setToIdentity();
+        o1.yaw(yaw);
+        o1.pitch(pitch);
+        o1.roll(roll);
+    
+        glm::vec3 front = o1.getFront();
+        glm::vec3 up    = o1.getUp();
+        glm::vec3 right = o1.getRight();
+
+        printf("\n-----\nTest: %d - yaw=%f , pitch=%f , roll=%f \n\n",i+1,yaw,pitch,roll);
+
+        printf(" +front.x=%f, front.y=%f, front.z=%f\n",front.x,front.y,front.z);
+        if (front.x == tests[i].frontX && front.y == tests[i].frontY && front.z == tests[i].frontZ) {
+            printf("  front vector PASSES!\n");
+        } else {
+            printf("  front vector FAILED! expected: \n");
+            printf("  front.x=%f, front.y=%f, front.z=%f\n",tests[i].frontX,tests[i].frontY,tests[i].frontZ);
+            passed = false;
+        }
+            
+        printf(" +up.x=%f,    up.y=%f,    up.z=%f\n",up.x,up.y,up.z);
+        if (up.x == tests[i].upX && up.y == tests[i].upY && up.z == tests[i].upZ) {
+            printf("  up vector PASSES!\n");
+        } else {
+            printf("  up vector FAILED! expected: \n");
+            printf("  up.x=%f, up.y=%f, up.z=%f\n",tests[i].upX,tests[i].upY,tests[i].upZ);
+            passed = false;
+        }
+
+
+        printf(" +right.x=%f, right.y=%f, right.z=%f\n",right.x,right.y,right.z);
+        if (right.x == tests[i].rightX && right.y == tests[i].rightY && right.z == tests[i].rightZ) {
+            printf("  right vector PASSES!\n");
+        } else {
+            printf("  right vector FAILED! expected: \n");
+            printf("  right.x=%f, right.y=%f, right.z=%f\n",tests[i].rightX,tests[i].rightY,tests[i].rightZ);
+            passed = false;
+        }
+        
+        if (!passed) {
+            printf("\n-----\nTest: %d - FAILED! \n----------\n\n",i+1);
+            failedCount++;
+        }
+    }
+    printf("\n-----\nTotal Failed: %d out of %d \n----------\n\n",failedCount,totalTests);
+    printf("\n----------DONE----------\n\n");
+}
+
+
+
+
+
+

interface/src/Util.h

@@ -17,20 +17,8 @@
 
 #include <glm/glm.hpp>
 
-static const float	ZERO				= 0.0;
+#include <Orientation.h>
-static const float	ONE					= 1.0;
+
-static const float	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 computations 
-static const double SQUARE_ROOT_OF_2	= sqrt(2);	
-static const double SQUARE_ROOT_OF_3	= sqrt(3);	
-static const float METER				= 1.0; 
-static const float DECIMETER			= 0.1; 
-static const float CENTIMETER			= 0.01; 
-static const float 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);
@@ -45,7 +33,43 @@ void drawvec3(int x, int y, float scale, float rotate, float thick, int mono, gl
               float r=1.0, float g=1.0, float b=1.0);
 double diffclock(timeval *clock1,timeval *clock2);
 
-
 void drawGroundPlaneGrid( float size, int resolution );
 
+void renderOrientationDirections( glm::vec3 position, Orientation orientation, float size );
+
+
+class oTestCase {
+public:
+    float yaw;
+    float pitch;
+    float roll;
+    
+    float frontX;    
+    float frontY;
+    float frontZ;    
+    
+    float upX;    
+    float upY;
+    float upZ;    
+    
+    float rightX;    
+    float rightY;
+    float rightZ;    
+    
+    oTestCase(
+        float yaw, float pitch, float roll, 
+        float frontX, float frontY, float frontZ,
+        float upX, float upY, float upZ,
+        float rightX, float rightY, float rightZ
+    ) : 
+        yaw(yaw),pitch(pitch),roll(roll),
+        frontX(frontX),frontY(frontY),frontZ(frontZ),
+        upX(upX),upY(upY),upZ(upZ),
+        rightX(rightX),rightY(rightY),rightZ(rightZ)
+    {};
+};
+
+
+void testOrientationClass();
+
 #endif

interface/src/VoxelSystem.cpp

@@ -112,33 +112,33 @@ long int VoxelSystem::getVoxelsBytesReadRunningAverage() {
 }
 
 
-void VoxelSystem::parseData(void *data, int size) {
+void VoxelSystem::parseData(unsigned char* sourceBuffer, int numBytes) {
 
-    unsigned char command = *(unsigned char*)data;
+    unsigned char command = *sourceBuffer;
-    unsigned char *voxelData = (unsigned char *) data + 1;
+    unsigned char *voxelData = sourceBuffer + 1;
     
     switch(command) {
         case PACKET_HEADER_VOXEL_DATA:
             // ask the VoxelTree to read the bitstream into the tree
-            tree->readBitstreamToTree(voxelData, size - 1);
+            tree->readBitstreamToTree(voxelData, numBytes - 1);
         break;
         case PACKET_HEADER_ERASE_VOXEL:
             // ask the tree to read the "remove" bitstream
-            tree->processRemoveVoxelBitstream((unsigned char*)data,size);
+            tree->processRemoveVoxelBitstream(sourceBuffer, numBytes);
         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* packetData = (char *)sourceBuffer;
             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);
+            printf("got Z message len(%d)= %s\n", numBytes, command);
 
-            while (totalLength <= size) {
+            while (totalLength <= numBytes) {
                 if (0==strcmp(command,(char*)"erase all")) {
                     printf("got Z message == erase all\n");
                     tree->eraseAllVoxels();
@@ -184,7 +184,7 @@ int VoxelSystem::treeToArrays(VoxelNode *currentNode, float nodePosition[3]) {
     int voxelsAdded = 0;
 
     float halfUnitForVoxel = powf(0.5, *currentNode->octalCode) * (0.5 * TREE_SCALE);
-    glm::vec3 viewerPosition = viewerHead->getPos();
+    glm::vec3 viewerPosition = viewerHead->getBodyPosition();
 
     // 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 

interface/src/VoxelSystem.h

@@ -26,7 +26,7 @@ public:
     VoxelSystem();
     ~VoxelSystem();
     
-    void parseData(void *data, int size);
+    void parseData(unsigned char* sourceBuffer, int numBytes);
     VoxelSystem* clone() const;
     
     void init();

interface/src/main.cpp

@@ -79,6 +79,9 @@
 
 using namespace std;
 
+void reshape(int width, int height); // will be defined below
+
+
 pthread_t networkReceiveThread;
 bool stopNetworkReceiveThread = false;
 
@@ -97,10 +100,13 @@ bool wantColorRandomizer = true;    // for addSphere and load file
 
 Oscilloscope audioScope(256,200,true);
 
+ViewFrustum viewFrustum;			// current state of view frustum, perspective, orientation, etc.
+
 Head myAvatar;                      // The rendered avatar of oneself
 Camera myCamera;                    // My view onto the world (sometimes on myself :)
+Camera viewFrustumOffsetCamera;     // The camera we use to sometimes show the view frustum from an offset mode
 
-                                    //  Starfield information
+//  Starfield information
 char starFile[] = "https://s3-us-west-1.amazonaws.com/highfidelity/stars.txt";
 FieldOfView fov;
 Stars stars;
@@ -136,10 +142,7 @@ Audio audio(&audioScope, &myAvatar);
 #define IDLE_SIMULATE_MSECS 8            //  How often should call simulate and other stuff 
                                          //  in the idle loop?
 
-float yaw = 0.f;                         //  The yaw, pitch for the avatar head
-float pitch = 0.f;                            
 float startYaw = 122.f;
-float renderPitch = 0.f;
 float renderYawRate = 0.f;
 float renderPitchRate = 0.f; 
 
@@ -225,7 +228,7 @@ void displayStats(void)
     char legend2[] = "* - toggle stars, & - toggle paint mode, '-' - send erase all, '%' - send add scene";
     drawtext(10, statsVerticalOffset + 32, 0.10f, 0, 1.0, 0, legend2);
 
-	glm::vec3 avatarPos = myAvatar.getPos();
+	glm::vec3 avatarPos = myAvatar.getBodyPosition();
     
     char stats[200];
     sprintf(stats, "FPS = %3.0f  Pkts/s = %d  Bytes/s = %d Head(x,y,z)= %4.2f, %4.2f, %4.2f ", 
@@ -311,7 +314,7 @@ void init(void)
     if (noiseOn) {   
         myAvatar.setNoise(noise);
     }
-    myAvatar.setPos(start_location );
+    myAvatar.setBodyPosition(start_location);
 	myCamera.setPosition( start_location );
     
 	
@@ -351,9 +354,9 @@ void reset_sensors()
     // 
     myAvatar.setRenderYaw(startYaw);
     
-    yaw = renderYawRate = 0; 
+    renderYawRate = 0; 
-    pitch = renderPitch = renderPitchRate = 0;
+    renderPitchRate = 0;
-    myAvatar.setPos(start_location);
+    myAvatar.setBodyPosition(start_location);
     headMouseX = WIDTH/2;
     headMouseY = HEIGHT/2;
     
@@ -364,7 +367,7 @@ void reset_sensors()
     }
 }
 
-void simulateHand(float deltaTime) {
+void updateAvatarHand(float deltaTime) {
     //  If mouse is being dragged, send current force to the hand controller
     if (mousePressed == 1)
     {
@@ -378,28 +381,26 @@ void simulateHand(float deltaTime) {
     }
 }
 
-void simulateHead(float frametime)
+//
-//  Using serial data, update avatar/render position and angles
+//  Using gyro data, update both view frustum and avatar head position
+//
+void updateAvatar(float frametime)
 {
-//    float measured_pitch_rate = serialPort.getRelativeValue(PITCH_RATE);
+    float gyroPitchRate = serialPort.getRelativeValue(PITCH_RATE);
-//    float measured_yaw_rate = serialPort.getRelativeValue(YAW_RATE);
+    float gyroYawRate = serialPort.getRelativeValue(YAW_RATE);
-    
-    float measured_pitch_rate = 0;
-    float measured_yaw_rate = 0;
-    
-    //float measured_lateral_accel = serialPort.getRelativeValue(ACCEL_X);
-    //float measured_fwd_accel = serialPort.getRelativeValue(ACCEL_Z);
     
     myAvatar.UpdatePos(frametime, &serialPort, headMirror, &gravity);
 		
-    //  Update head_mouse model 
+    //  
+    //  Update gyro-based mouse (X,Y on screen)
+    // 
     const float MIN_MOUSE_RATE = 30.0;
     const float MOUSE_SENSITIVITY = 0.1f;
-    if (powf(measured_yaw_rate*measured_yaw_rate + 
+    if (powf(gyroYawRate*gyroYawRate + 
-             measured_pitch_rate*measured_pitch_rate, 0.5) > MIN_MOUSE_RATE)
+             gyroPitchRate*gyroPitchRate, 0.5) > MIN_MOUSE_RATE)
     {
-        headMouseX += measured_yaw_rate*MOUSE_SENSITIVITY;
+        headMouseX += gyroYawRate*MOUSE_SENSITIVITY;
-        headMouseY += measured_pitch_rate*MOUSE_SENSITIVITY*(float)HEIGHT/(float)WIDTH; 
+        headMouseY += gyroPitchRate*MOUSE_SENSITIVITY*(float)HEIGHT/(float)WIDTH; 
     }
     headMouseX = max(headMouseX, 0);
     headMouseX = min(headMouseX, WIDTH);
@@ -409,7 +410,6 @@ void simulateHead(float frametime)
     //  Update render direction (pitch/yaw) based on measured gyro rates
     const int MIN_YAW_RATE = 100;
     const int MIN_PITCH_RATE = 100;
-    
     const float YAW_SENSITIVITY = 0.02;
     const float PITCH_SENSITIVITY = 0.05;
     
@@ -418,23 +418,22 @@ void simulateHead(float frametime)
     if (myAvatar.getDriveKeys(ROT_LEFT)) renderYawRate -= KEY_YAW_SENSITIVITY*frametime;
     if (myAvatar.getDriveKeys(ROT_RIGHT)) renderYawRate += KEY_YAW_SENSITIVITY*frametime;
     
-    if (fabs(measured_yaw_rate) > MIN_YAW_RATE)  
+    if (fabs(gyroYawRate) > MIN_YAW_RATE)  
     {   
-        if (measured_yaw_rate > 0)
+        if (gyroYawRate > 0)
-            renderYawRate += (measured_yaw_rate - MIN_YAW_RATE) * YAW_SENSITIVITY * frametime;
+            renderYawRate += (gyroYawRate - MIN_YAW_RATE) * YAW_SENSITIVITY * frametime;
         else 
-            renderYawRate += (measured_yaw_rate + MIN_YAW_RATE) * YAW_SENSITIVITY * frametime;
+            renderYawRate += (gyroYawRate + MIN_YAW_RATE) * YAW_SENSITIVITY * frametime;
     }
-    if (fabs(measured_pitch_rate) > MIN_PITCH_RATE) 
+    if (fabs(gyroPitchRate) > MIN_PITCH_RATE) 
     {
-        if (measured_pitch_rate > 0)
+        if (gyroPitchRate > 0)
-            renderPitchRate += (measured_pitch_rate - MIN_PITCH_RATE) * PITCH_SENSITIVITY * frametime;
+            renderPitchRate += (gyroPitchRate - MIN_PITCH_RATE) * PITCH_SENSITIVITY * frametime;
         else 
-            renderPitchRate += (measured_pitch_rate + MIN_PITCH_RATE) * PITCH_SENSITIVITY * frametime;
+            renderPitchRate += (gyroPitchRate + MIN_PITCH_RATE) * PITCH_SENSITIVITY * frametime;
     }
          
-    renderPitch += renderPitchRate;
+    float renderPitch = myAvatar.getRenderPitch();
-    
     // Decay renderPitch toward zero because we never look constantly up/down 
     renderPitch *= (1.f - 2.0*frametime);
 
@@ -442,9 +441,9 @@ void simulateHead(float frametime)
     renderPitchRate *= (1.f - 5.0*frametime);
     renderYawRate *= (1.f - 7.0*frametime);
     
-    //  Update own head data
+    //  Update own avatar data
     myAvatar.setRenderYaw(myAvatar.getRenderYaw() + renderYawRate);
-    myAvatar.setRenderPitch(renderPitch);
+    myAvatar.setRenderPitch(renderPitch + renderPitchRate);
     
     //  Get audio loudness data from audio input device
     float loudness, averageLoudness;
@@ -455,8 +454,10 @@ void simulateHead(float frametime)
     #endif
 
     //  Send my stream of head/hand data to the avatar mixer and voxel server
-    char broadcastString[200];
+    unsigned char broadcastString[200];
-    int broadcastBytes = myAvatar.getBroadcastData(broadcastString);
+    *broadcastString = PACKET_HEADER_HEAD_DATA;
+    
+    int broadcastBytes = myAvatar.getBroadcastData(broadcastString + 1);
     const char broadcastReceivers[2] = {AGENT_TYPE_VOXEL, AGENT_TYPE_AVATAR_MIXER};
     
     AgentList::getInstance()->broadcastToAgents(broadcastString, broadcastBytes, broadcastReceivers, 2);
@@ -464,7 +465,7 @@ void simulateHead(float frametime)
     // If I'm in paint mode, send a voxel out to VOXEL server agents.
     if (::paintOn) {
     
-    	glm::vec3 avatarPos = myAvatar.getPos();
+    	glm::vec3 avatarPos = myAvatar.getBodyPosition();
 
 		// For some reason, we don't want to flip X and Z here.
 		::paintingVoxel.x = avatarPos.x/10.0;  
@@ -479,7 +480,7 @@ void simulateHead(float frametime)
 			::paintingVoxel.z >= 0.0 && ::paintingVoxel.z <= 1.0) {
 
 			if (createVoxelEditMessage(PACKET_HEADER_SET_VOXEL, 0, 1, &::paintingVoxel, bufferOut, sizeOut)){
-                AgentList::getInstance()->broadcastToAgents((char*)bufferOut, sizeOut, &AGENT_TYPE_VOXEL, 1);
+                AgentList::getInstance()->broadcastToAgents(bufferOut, sizeOut, &AGENT_TYPE_VOXEL, 1);
 				delete bufferOut;
 			}
 		}
@@ -519,48 +520,66 @@ int   frustumDrawingMode = FRUSTUM_DRAW_MODE_ALL; // the mode we're drawing the
 bool  frustumOn = false;                  // Whether or not to display the debug view frustum
 bool  cameraFrustum = true;               // which frustum to look at
 
-bool  viewFrustumFromOffset=false;        // Wether or not to offset the view of the frustum
+bool  viewFrustumFromOffset     =false;   // Wether or not to offset the view of the frustum
-float viewFrustumOffsetYaw = -90.0;       // the following variables control yaw, pitch, roll and distance form regular
+float viewFrustumOffsetYaw      = -135.0; // the following variables control yaw, pitch, roll and distance form regular
-float viewFrustumOffsetPitch = 7.5;       // camera to the offset camera
+float viewFrustumOffsetPitch    = 0.0;    // camera to the offset camera
 float viewFrustumOffsetRoll     = 0.0;
-float viewFrustumOffsetDistance = 0.0;
+float viewFrustumOffsetDistance = 25.0;
 float viewFrustumOffsetUp       = 0.0;
 
 void render_view_frustum() {
 	
-	glm::vec3 cameraPosition   = ::myCamera.getPosition(); 
-	glm::vec3 headPosition     = ::myAvatar.getHeadPosition();
-
-	glm::vec3 cameraDirection = ::myCamera.getOrientation().getFront() * glm::vec3(1,1,-1);
-	glm::vec3 headDirection    = myAvatar.getHeadLookatDirection(); // direction still backwards
-
-	glm::vec3 cameraUp  = myCamera.getOrientation().getUp() * glm::vec3(1,1,1);
-	glm::vec3 headUp    = myAvatar.getHeadLookatDirectionUp();
-
-	glm::vec3 cameraRight = myCamera.getOrientation().getRight() * glm::vec3(1,1,-1);
-	glm::vec3 headRight = myAvatar.getHeadLookatDirectionRight() * glm::vec3(-1,1,-1); // z is flipped!
-
 	// We will use these below, from either the camera or head vectors calculated above	
 	glm::vec3 position;
 	glm::vec3 direction;
 	glm::vec3 up;
 	glm::vec3 right;
+	float fov, nearClip, farClip;
 	
 	// Camera or Head?
 	if (::cameraFrustum) {
-		position = cameraPosition;
+		position    = ::myCamera.getPosition();
-		direction = cameraDirection;
+		direction   = ::myCamera.getOrientation().getFront() * glm::vec3(1,1,-1);
-		up = cameraUp;
+		up          = ::myCamera.getOrientation().getUp() * glm::vec3(1,1,1);
-		right = cameraRight;
+		right       = ::myCamera.getOrientation().getRight() * glm::vec3(1,1,-1);
+		fov         = ::myCamera.getFieldOfView();
+		nearClip    = ::myCamera.getNearClip();
+		farClip     = ::myCamera.getFarClip();
 	} else {
-		position = headPosition;
+		position    = ::myAvatar.getHeadPosition();
-		direction = headDirection;
+		direction   = ::myAvatar.getHeadLookatDirection();
-		up = headUp;
+		up          = ::myAvatar.getHeadLookatDirectionUp();
-		right = headRight;
+		right       = ::myAvatar.getHeadLookatDirectionRight() * glm::vec3(-1,1,-1);
+		
+		// NOTE: we use the same lens details if we draw from the head
+		fov         = ::myCamera.getFieldOfView();
+		nearClip    = ::myCamera.getNearClip();
+		farClip     = ::myCamera.getFarClip();
 	}
 
+    /*
+    printf("position.x=%f, position.y=%f, position.z=%f\n", position.x, position.y, position.z);
+    printf("direction.x=%f, direction.y=%f, direction.z=%f\n", direction.x, direction.y, direction.z);
+    printf("up.x=%f, up.y=%f, up.z=%f\n", up.x, up.y, up.z);
+    printf("right.x=%f, right.y=%f, right.z=%f\n", right.x, right.y, right.z);
+    printf("fov=%f\n", fov);
+    printf("nearClip=%f\n", nearClip);
+    printf("farClip=%f\n", farClip);
+    */
+
+    // Set the viewFrustum up with the correct position and orientation of the camera	
+    viewFrustum.setPosition(position);
+    viewFrustum.setOrientation(direction,up,right);
+    
+    // Also make sure it's got the correct lens details from the camera
+    viewFrustum.setFieldOfView(fov);
+    viewFrustum.setNearClip(nearClip);
+    viewFrustum.setFarClip(farClip);
+
     // Ask the ViewFrustum class to calculate our corners
-    ViewFrustum vf(position,direction,up,right,::WIDTH,::HEIGHT);
+    viewFrustum.calculate();
+    
+    //viewFrustum.dump();
 	
     //  Get ready to draw some lines
     glDisable(GL_LIGHTING);
@@ -593,64 +612,64 @@ void render_view_frustum() {
 	if (::frustumDrawingMode == FRUSTUM_DRAW_MODE_ALL || ::frustumDrawingMode == FRUSTUM_DRAW_MODE_PLANES
 			|| ::frustumDrawingMode == FRUSTUM_DRAW_MODE_NEAR_PLANE) {
 		// Drawing the bounds of the frustum
-		// vf.getNear plane - bottom edge 
+		// viewFrustum.getNear plane - bottom edge 
 		glColor3f(1,0,0);
-		glVertex3f(vf.getNearBottomLeft().x, vf.getNearBottomLeft().y, vf.getNearBottomLeft().z);
+		glVertex3f(viewFrustum.getNearBottomLeft().x, viewFrustum.getNearBottomLeft().y, viewFrustum.getNearBottomLeft().z);
-		glVertex3f(vf.getNearBottomRight().x, vf.getNearBottomRight().y, vf.getNearBottomRight().z);
+		glVertex3f(viewFrustum.getNearBottomRight().x, viewFrustum.getNearBottomRight().y, viewFrustum.getNearBottomRight().z);
 
-		// vf.getNear plane - top edge
+		// viewFrustum.getNear plane - top edge
-		glVertex3f(vf.getNearTopLeft().x, vf.getNearTopLeft().y, vf.getNearTopLeft().z);
+		glVertex3f(viewFrustum.getNearTopLeft().x, viewFrustum.getNearTopLeft().y, viewFrustum.getNearTopLeft().z);
-		glVertex3f(vf.getNearTopRight().x, vf.getNearTopRight().y, vf.getNearTopRight().z);
+		glVertex3f(viewFrustum.getNearTopRight().x, viewFrustum.getNearTopRight().y, viewFrustum.getNearTopRight().z);
 
-		// vf.getNear plane - right edge
+		// viewFrustum.getNear plane - right edge
-		glVertex3f(vf.getNearBottomRight().x, vf.getNearBottomRight().y, vf.getNearBottomRight().z);
+		glVertex3f(viewFrustum.getNearBottomRight().x, viewFrustum.getNearBottomRight().y, viewFrustum.getNearBottomRight().z);
-		glVertex3f(vf.getNearTopRight().x, vf.getNearTopRight().y, vf.getNearTopRight().z);
+		glVertex3f(viewFrustum.getNearTopRight().x, viewFrustum.getNearTopRight().y, viewFrustum.getNearTopRight().z);
 
-		// vf.getNear plane - left edge
+		// viewFrustum.getNear plane - left edge
-		glVertex3f(vf.getNearBottomLeft().x, vf.getNearBottomLeft().y, vf.getNearBottomLeft().z);
+		glVertex3f(viewFrustum.getNearBottomLeft().x, viewFrustum.getNearBottomLeft().y, viewFrustum.getNearBottomLeft().z);
-		glVertex3f(vf.getNearTopLeft().x, vf.getNearTopLeft().y, vf.getNearTopLeft().z);
+		glVertex3f(viewFrustum.getNearTopLeft().x, viewFrustum.getNearTopLeft().y, viewFrustum.getNearTopLeft().z);
 	}
 
 	if (::frustumDrawingMode == FRUSTUM_DRAW_MODE_ALL || ::frustumDrawingMode == FRUSTUM_DRAW_MODE_PLANES
 			|| ::frustumDrawingMode == FRUSTUM_DRAW_MODE_FAR_PLANE) {
-		// vf.getFar plane - bottom edge 
+		// viewFrustum.getFar plane - bottom edge 
 		glColor3f(0,1,0); // GREEN!!!
-		glVertex3f(vf.getFarBottomLeft().x, vf.getFarBottomLeft().y, vf.getFarBottomLeft().z);
+		glVertex3f(viewFrustum.getFarBottomLeft().x, viewFrustum.getFarBottomLeft().y, viewFrustum.getFarBottomLeft().z);
-		glVertex3f(vf.getFarBottomRight().x, vf.getFarBottomRight().y, vf.getFarBottomRight().z);
+		glVertex3f(viewFrustum.getFarBottomRight().x, viewFrustum.getFarBottomRight().y, viewFrustum.getFarBottomRight().z);
 
-		// vf.getFar plane - top edge
+		// viewFrustum.getFar plane - top edge
-		glVertex3f(vf.getFarTopLeft().x, vf.getFarTopLeft().y, vf.getFarTopLeft().z);
+		glVertex3f(viewFrustum.getFarTopLeft().x, viewFrustum.getFarTopLeft().y, viewFrustum.getFarTopLeft().z);
-		glVertex3f(vf.getFarTopRight().x, vf.getFarTopRight().y, vf.getFarTopRight().z);
+		glVertex3f(viewFrustum.getFarTopRight().x, viewFrustum.getFarTopRight().y, viewFrustum.getFarTopRight().z);
 
-		// vf.getFar plane - right edge
+		// viewFrustum.getFar plane - right edge
-		glVertex3f(vf.getFarBottomRight().x, vf.getFarBottomRight().y, vf.getFarBottomRight().z);
+		glVertex3f(viewFrustum.getFarBottomRight().x, viewFrustum.getFarBottomRight().y, viewFrustum.getFarBottomRight().z);
-		glVertex3f(vf.getFarTopRight().x, vf.getFarTopRight().y, vf.getFarTopRight().z);
+		glVertex3f(viewFrustum.getFarTopRight().x, viewFrustum.getFarTopRight().y, viewFrustum.getFarTopRight().z);
 
-		// vf.getFar plane - left edge
+		// viewFrustum.getFar plane - left edge
-		glVertex3f(vf.getFarBottomLeft().x, vf.getFarBottomLeft().y, vf.getFarBottomLeft().z);
+		glVertex3f(viewFrustum.getFarBottomLeft().x, viewFrustum.getFarBottomLeft().y, viewFrustum.getFarBottomLeft().z);
-		glVertex3f(vf.getFarTopLeft().x, vf.getFarTopLeft().y, vf.getFarTopLeft().z);
+		glVertex3f(viewFrustum.getFarTopLeft().x, viewFrustum.getFarTopLeft().y, viewFrustum.getFarTopLeft().z);
 	}
 
 	if (::frustumDrawingMode == FRUSTUM_DRAW_MODE_ALL || ::frustumDrawingMode == FRUSTUM_DRAW_MODE_PLANES) {
 		// RIGHT PLANE IS CYAN
-		// right plane - bottom edge - vf.getNear to distant 
+		// right plane - bottom edge - viewFrustum.getNear to distant 
 		glColor3f(0,1,1);
-		glVertex3f(vf.getNearBottomRight().x, vf.getNearBottomRight().y, vf.getNearBottomRight().z);
+		glVertex3f(viewFrustum.getNearBottomRight().x, viewFrustum.getNearBottomRight().y, viewFrustum.getNearBottomRight().z);
-		glVertex3f(vf.getFarBottomRight().x, vf.getFarBottomRight().y, vf.getFarBottomRight().z);
+		glVertex3f(viewFrustum.getFarBottomRight().x, viewFrustum.getFarBottomRight().y, viewFrustum.getFarBottomRight().z);
 
-		// right plane - top edge - vf.getNear to distant
+		// right plane - top edge - viewFrustum.getNear to distant
-		glVertex3f(vf.getNearTopRight().x, vf.getNearTopRight().y, vf.getNearTopRight().z);
+		glVertex3f(viewFrustum.getNearTopRight().x, viewFrustum.getNearTopRight().y, viewFrustum.getNearTopRight().z);
-		glVertex3f(vf.getFarTopRight().x, vf.getFarTopRight().y, vf.getFarTopRight().z);
+		glVertex3f(viewFrustum.getFarTopRight().x, viewFrustum.getFarTopRight().y, viewFrustum.getFarTopRight().z);
 
 		// LEFT PLANE IS BLUE
-		// left plane - bottom edge - vf.getNear to distant
+		// left plane - bottom edge - viewFrustum.getNear to distant
 		glColor3f(0,0,1);
-		glVertex3f(vf.getNearBottomLeft().x, vf.getNearBottomLeft().y, vf.getNearBottomLeft().z);
+		glVertex3f(viewFrustum.getNearBottomLeft().x, viewFrustum.getNearBottomLeft().y, viewFrustum.getNearBottomLeft().z);
-		glVertex3f(vf.getFarBottomLeft().x, vf.getFarBottomLeft().y, vf.getFarBottomLeft().z);
+		glVertex3f(viewFrustum.getFarBottomLeft().x, viewFrustum.getFarBottomLeft().y, viewFrustum.getFarBottomLeft().z);
 
-		// left plane - top edge - vf.getNear to distant
+		// left plane - top edge - viewFrustum.getNear to distant
-		glVertex3f(vf.getNearTopLeft().x, vf.getNearTopLeft().y, vf.getNearTopLeft().z);
+		glVertex3f(viewFrustum.getNearTopLeft().x, viewFrustum.getNearTopLeft().y, viewFrustum.getNearTopLeft().z);
-		glVertex3f(vf.getFarTopLeft().x, vf.getFarTopLeft().y, vf.getFarTopLeft().z);
+		glVertex3f(viewFrustum.getFarTopLeft().x, viewFrustum.getFarTopLeft().y, viewFrustum.getFarTopLeft().z);
 	}
 
     glEnd();
@@ -660,8 +679,6 @@ void render_view_frustum() {
 
 void display(void)
 {
-	//printf( "avatar head lookat = %f, %f, %f\n", myAvatar.getAvatarHeadLookatDirection().x, myAvatar.getAvatarHeadLookatDirection().y, myAvatar.getAvatarHeadLookatDirection().z );
-
 	PerfStat("display");
 
     glEnable(GL_LINE_SMOOTH);
@@ -690,31 +707,34 @@ void display(void)
 		//--------------------------------------------------------
 		// camera settings
 		//--------------------------------------------------------		
+		myCamera.setTargetPosition( myAvatar.getBodyPosition() );
+
 		if ( displayHead ) {
 			//-----------------------------------------------
 			// set the camera to looking at my own face
 			//-----------------------------------------------
-			myCamera.setTargetPosition	( myAvatar.getPos() ); 
+			myCamera.setTargetPosition	( myAvatar.getBodyPosition() ); // XXXBHG - Shouldn't we use Head position here?
 			myCamera.setYaw				( - myAvatar.getBodyYaw() );
 			myCamera.setPitch			( 0.0  );
 			myCamera.setRoll			( 0.0  );
-			myCamera.setUp				( 0.53 );	
+			myCamera.setUp				( 0.6 );	
-			myCamera.setDistance		( 0.03 );
+			myCamera.setDistance		( 0.3  );
 			myCamera.setTightness		( 100.0f );
 			myCamera.update				( 1.f/FPS );
 		} else {
 			//----------------------------------------------------
 			// set the camera to third-person view behind my av
 			//----------------------------------------------------		
-			myCamera.setTargetPosition	( myAvatar.getPos() ); 
+			myCamera.setTargetPosition	( myAvatar.getBodyPosition() );
 			myCamera.setYaw				( 180.0 - myAvatar.getBodyYaw() );
-			myCamera.setPitch			(  10.0 );
+			myCamera.setPitch			(  10.0 );  // temporarily, this must be 0.0 or else bad juju
 			myCamera.setRoll			(   0.0 );
-			myCamera.setUp				(   0.45 );
+			myCamera.setUp				(   0.45);
-			myCamera.setDistance		(   0.5 );
+			myCamera.setDistance		(   1.0 );
 			myCamera.setTightness		( 10.0f );
 			myCamera.update				( 1.f/FPS );
 		}
+		
 		// Note: whichCamera is used to pick between the normal camera myCamera for our 
 		// main camera, vs, an alternate camera. The alternate camera we support right now
 		// is the viewFrustumOffsetCamera. But theoretically, we could use this same mechanism
@@ -727,29 +747,35 @@ void display(void)
 		Camera viewFrustumOffsetCamera = myCamera;
 
 		if (::viewFrustumFromOffset && ::frustumOn) {
-			//----------------------------------------------------
-			// set the camera to third-person view but offset so we can see the frustum
-			//----------------------------------------------------		
-			viewFrustumOffsetCamera.setYaw		( 180.0 - myAvatar.getBodyYaw() + ::viewFrustumOffsetYaw );
-			viewFrustumOffsetCamera.setPitch	(   0.0 + ::viewFrustumOffsetPitch );
-			viewFrustumOffsetCamera.setRoll	(   0.0 + ::viewFrustumOffsetRoll  ); 
-			viewFrustumOffsetCamera.setUp		(   0.2 + 0.2 );
-			viewFrustumOffsetCamera.setDistance(   0.5 + 0.2 );
-			viewFrustumOffsetCamera.update( 1.f/FPS );
 
+			// set the camera to third-person view but offset so we can see the frustum
+			viewFrustumOffsetCamera.setYaw		(  180.0 - myAvatar.getBodyYaw() + ::viewFrustumOffsetYaw );
+			viewFrustumOffsetCamera.setPitch	(  ::viewFrustumOffsetPitch    );
+			viewFrustumOffsetCamera.setRoll     (  ::viewFrustumOffsetRoll     ); 
+			viewFrustumOffsetCamera.setUp		(  ::viewFrustumOffsetUp       );
+			viewFrustumOffsetCamera.setDistance (  ::viewFrustumOffsetDistance );
+			viewFrustumOffsetCamera.update(1.f/FPS);
 			whichCamera = viewFrustumOffsetCamera;
 		}		
 
-		//---------------------------------------------
 		// transform view according to whichCamera
 		// could be myCamera (if in normal mode)
 		// or could be viewFrustumOffsetCamera if in offset mode
-		//---------------------------------------------
+		// I changed the ordering here - roll is FIRST (JJV) 
+        glRotatef	( whichCamera.getRoll(),	0, 0, 1 );
         glRotatef	( whichCamera.getPitch(),	1, 0, 0 );
         glRotatef	( whichCamera.getYaw(),	    0, 1, 0 );
-        glRotatef	( whichCamera.getRoll(),	0, 0, 1 );
         glTranslatef( -whichCamera.getPosition().x, -whichCamera.getPosition().y, -whichCamera.getPosition().z );
 
+
+
+
+        //quick test for camera ortho-normal sanity check...
+        
+
+
+
+
         if (::starsOn) {
             // should be the first rendering pass - w/o depth buffer / lighting
         	stars.render(fov);
@@ -803,7 +829,7 @@ void display(void)
             if (agent->getLinkedData() != NULL) {
                 Head *agentHead = (Head *)agent->getLinkedData();
                 glPushMatrix();
-                glm::vec3 pos = agentHead->getPos();
+                glm::vec3 pos = agentHead->getBodyPosition();
                 glTranslatef(-pos.x, -pos.y, -pos.z);
                 agentHead->render(0, 0);
                 glPopMatrix();
@@ -893,9 +919,11 @@ void display(void)
     
     //  If application has just started, report time from startup to now (first frame display)
     if (justStarted) {
-        printf("Startup Time: %4.2f\n",
+        float startupTime = (usecTimestampNow() - usecTimestamp(&applicationStartupTime))/1000000.0;
-               (usecTimestampNow() - usecTimestamp(&applicationStartupTime))/1000000.0);
         justStarted = false;
+        char title[30];
+        snprintf(title, 30, "Interface: %4.2f seconds", startupTime);
+        glutSetWindowTitle(title);
     }
 }
 
@@ -966,7 +994,14 @@ int setDisplayFrustum(int state) {
 }
 
 int setFrustumOffset(int state) {
-    return setValue(state, &::viewFrustumFromOffset);
+    int value = setValue(state, &::viewFrustumFromOffset);
+
+    // reshape so that OpenGL will get the right lens details for the camera of choice    
+    if (state == MENU_ROW_PICKED) {
+        reshape(::WIDTH,::HEIGHT);
+    }
+    
+    return value;
 }
 
 int setFrustumOrigin(int state) {
@@ -1026,12 +1061,13 @@ void initMenu() {
     menuColumnOptions->addRow("(V)oxels", setVoxels);
     menuColumnOptions->addRow("Stars (*)", setStars);
     menuColumnOptions->addRow("(Q)uit", quitApp);
+
     //  Tools
     menuColumnTools = menu.addColumn("Tools");
     menuColumnTools->addRow("Stats (/)", setStats); 
     menuColumnTools->addRow("(M)enu", setMenu);
 
-    // Debug
+    // Frustum Options
     menuColumnFrustum = menu.addColumn("Frustum");
     menuColumnFrustum->addRow("Display (F)rustum", setDisplayFrustum); 
     menuColumnFrustum->addRow("Use (O)ffset Camera", setFrustumOffset); 
@@ -1067,14 +1103,14 @@ void sendVoxelServerEraseAll() {
 	char message[100];
     sprintf(message,"%c%s",'Z',"erase all");
 	int messageSize = strlen(message) + 1;
-	AgentList::getInstance()->broadcastToAgents(message, messageSize, &AGENT_TYPE_VOXEL, 1);
+	AgentList::getInstance()->broadcastToAgents((unsigned char*) message, messageSize, &AGENT_TYPE_VOXEL, 1);
-}
+}\
 
 void sendVoxelServerAddScene() {
 	char message[100];
     sprintf(message,"%c%s",'Z',"add scene");
 	int messageSize = strlen(message) + 1;
-	AgentList::getInstance()->broadcastToAgents(message, messageSize, &AGENT_TYPE_VOXEL, 1);
+	AgentList::getInstance()->broadcastToAgents((unsigned char*)message, messageSize, &AGENT_TYPE_VOXEL, 1);
 }
 
 void shiftPaintingColor()
@@ -1087,7 +1123,7 @@ void shiftPaintingColor()
 }
 
 void setupPaintingVoxel() {
-	glm::vec3 avatarPos = myAvatar.getPos();
+	glm::vec3 avatarPos = myAvatar.getBodyPosition();
 
 	::paintingVoxel.x = avatarPos.z/-10.0;	// voxel space x is negative z head space
 	::paintingVoxel.y = avatarPos.y/-10.0;  // voxel space y is negative y head space
@@ -1187,7 +1223,7 @@ void key(unsigned char k, int x, int y)
     if (k == 'V' || k == 'v')  ::showingVoxels = !::showingVoxels;		// toggle voxels
     if (k == 'F')  ::frustumOn = !::frustumOn;		// toggle view frustum debugging
     if (k == 'C')  ::cameraFrustum = !::cameraFrustum;	// toggle which frustum to look at
-    if (k == 'O' || k == 'G')  ::viewFrustumFromOffset = !::viewFrustumFromOffset; // toggle view frustum offset debugging
+    if (k == 'O' || k == 'G') setFrustumOffset(MENU_ROW_PICKED); // toggle view frustum offset debugging
     
 	if (k == '[') ::viewFrustumOffsetYaw       -= 0.5;
 	if (k == ']') ::viewFrustumOffsetYaw       += 0.5;
@@ -1199,8 +1235,10 @@ void key(unsigned char k, int x, int y)
 	if (k == '>') ::viewFrustumOffsetDistance  += 0.5;
 	if (k == ',') ::viewFrustumOffsetUp        -= 0.05;
 	if (k == '.') ::viewFrustumOffsetUp        += 0.05;
-	if (k == '|') ViewFrustum::fovAngleAdust   -= 0.05;
+
-	if (k == '\\') ViewFrustum::fovAngleAdust  += 0.05;
+//	if (k == '|') ViewFrustum::fovAngleAdust   -= 0.05;
+//	if (k == '\\') ViewFrustum::fovAngleAdust  += 0.05;
+
 	if (k == 'R') setFrustumRenderMode(MENU_ROW_PICKED);
 	
     if (k == '&') {
@@ -1258,7 +1296,7 @@ void *networkReceive(void *args)
 {    
     sockaddr senderAddress;
     ssize_t bytesReceived;
-    char *incomingPacket = new char[MAX_PACKET_SIZE];
+    unsigned char *incomingPacket = new unsigned char[MAX_PACKET_SIZE];
 
     while (!stopNetworkReceiveThread) {
         if (AgentList::getInstance()->getAgentSocket().receive(&senderAddress, incomingPacket, &bytesReceived)) {
@@ -1275,7 +1313,10 @@ void *networkReceive(void *args)
                     voxels.parseData(incomingPacket, bytesReceived);
                     break;
                 case PACKET_HEADER_BULK_AVATAR_DATA:
-                    AgentList::getInstance()->processBulkAgentData(&senderAddress, incomingPacket, bytesReceived, sizeof(float) * 11);
+                    AgentList::getInstance()->processBulkAgentData(&senderAddress,
+                                                                   incomingPacket,
+                                                                   bytesReceived,
+                                                                   (sizeof(float) * 3) + (sizeof(uint16_t) * 2));
                     break;
                 default:
                     AgentList::getInstance()->processAgentData(&senderAddress, incomingPacket, bytesReceived);
@@ -1324,8 +1365,10 @@ void idle(void) {
 			myAvatar.setTriggeringAction( false );
 		}
 		
-        //  Simulation
+        //
-        simulateHead( 1.f/FPS );
+        //  Sample hardware, update view frustum if needed, send avatar data to mixer/agents
+        //
+        updateAvatar( 1.f/FPS );
 		
 		
 		//test
@@ -1340,7 +1383,7 @@ void idle(void) {
         }
 		*/
 		
-        simulateHand(1.f/FPS);
+        updateAvatarHand(1.f/FPS);
     
         field.simulate(1.f/FPS);
         myAvatar.simulate(1.f/FPS);
@@ -1359,27 +1402,66 @@ void idle(void) {
 }
 
 
-
 void reshape(int width, int height)
 {
     WIDTH = width;
     HEIGHT = height; 
+    float aspectRatio = ((float)width/(float)height); // based on screen resize
 
+    float fov;
+    float nearClip;
+    float farClip;
+
+    // get the lens details from the current camera
+    if (::viewFrustumFromOffset) {
+        fov       = ::viewFrustumOffsetCamera.getFieldOfView();
+        nearClip  = ::viewFrustumOffsetCamera.getNearClip();
+        farClip   = ::viewFrustumOffsetCamera.getFarClip();
+    } else {
+        fov       = ::myCamera.getFieldOfView();
+        nearClip  = ::myCamera.getNearClip();
+        farClip   = ::myCamera.getFarClip();
+    }
+
+    //printf("reshape() width=%d, height=%d, aspectRatio=%f fov=%f near=%f far=%f \n",
+    //    width,height,aspectRatio,fov,nearClip,farClip);
+    
+    // Tell our viewFrustum about this change
+    ::viewFrustum.setAspectRatio(aspectRatio);
+
+    
+    glViewport(0, 0, width, height); // shouldn't this account for the menu???
 
     glMatrixMode(GL_PROJECTION); //hello
-    fov.setResolution(width, height)
+
-            .setBounds(glm::vec3(-0.5f,-0.5f,-500.0f), glm::vec3(0.5f, 0.5f, 0.1f) )
+    // XXXBHG - Note: this is Tobias's code for loading the perspective matrix. At Philip's suggestion, I'm removing
-            .setPerspective(0.7854f);
+    // it and putting back our old code that simply loaded the fov, ratio, and near/far clips. But I'm keeping this here
-    glLoadMatrixf(glm::value_ptr(fov.getViewerScreenXform()));
+    // for reference for now.
+    //fov.setResolution(width, height)
+    //        .setBounds(glm::vec3(-0.5f,-0.5f,-500.0f), glm::vec3(0.5f, 0.5f, 0.1f) )
+    //        .setPerspective(0.7854f);
+    //glLoadMatrixf(glm::value_ptr(fov.getViewerScreenXform()));
+
+    glLoadIdentity();
+    
+    // XXXBHG - If we're in view frustum mode, then we need to do this little bit of hackery so that
+    // OpenGL won't clip our frustum rendering lines. This is a debug hack for sure! Basically, this makes
+    // the near clip a little bit closer (therefor you see more) and the far clip a little bit farther (also,
+    // to see more.)
+    if (::frustumOn) {
+        nearClip -= 0.01f;
+        farClip  += 0.01f;
+    }
+    
+    // On window reshape, we need to tell OpenGL about our new setting
+    gluPerspective(fov,aspectRatio,nearClip,farClip);
     
     glMatrixMode(GL_MODELVIEW);
     glLoadIdentity();
 
-    glViewport(0, 0, width, height);
+     
 }
 
-
-
 void mouseFunc( int button, int state, int x, int y ) 
 {
     if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN )
@@ -1429,6 +1511,9 @@ void audioMixerUpdate(in_addr_t newMixerAddress, in_port_t newMixerPort) {
 
 int main(int argc, const char * argv[])
 {
+    // Quick test of the Orientation class on startup!
+    testOrientationClass();    
+
     AgentList::createInstance(AGENT_TYPE_INTERFACE);
     
     gettimeofday(&applicationStartupTime, NULL);
@@ -1462,6 +1547,9 @@ int main(int argc, const char * argv[])
     AgentList::getInstance()->startPingUnknownAgentsThread();
 
     glutInit(&argc, (char**)argv);
+    WIDTH = glutGet(GLUT_SCREEN_WIDTH);
+    HEIGHT = glutGet(GLUT_SCREEN_HEIGHT);
+    
     glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
     glutInitWindowSize(WIDTH, HEIGHT);
     glutCreateWindow("Interface");
@@ -1470,6 +1558,12 @@ int main(int argc, const char * argv[])
     glewInit();
     #endif
     
+    // Before we render anything, let's set up our viewFrustumOffsetCamera with a sufficiently large
+    // field of view and near and far clip to make it interesting.
+    //viewFrustumOffsetCamera.setFieldOfView(90.0);
+    viewFrustumOffsetCamera.setNearClip(0.1);
+    viewFrustumOffsetCamera.setFarClip(500.0);
+
     printf( "Created Display Window.\n" );
         
     initMenu();
@@ -1514,4 +1608,3 @@ int main(int argc, const char * argv[])
     ::terminate();
     return EXIT_SUCCESS;
 }   
-

libraries/avatars/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8)
+
+set(ROOT_DIR ../..)
+set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
+
+# setup for find modules
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../../cmake/modules/")
+
+set(TARGET_NAME avatars)
+
+include(${MACRO_DIR}/SetupHifiLibrary.cmake)
+setup_hifi_library(${TARGET_NAME})
+
+include(${MACRO_DIR}/IncludeGLM.cmake)
+include_glm(${TARGET_NAME} ${ROOT_DIR})
+
+include(${MACRO_DIR}/LinkHifiLibrary.cmake)
+link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR})

libraries/avatars/src/AvatarData.cpp

@@ -0,0 +1,112 @@
+//
+//  AvatarData.cpp
+//  hifi
+//
+//  Created by Stephen Birarda on 4/9/13.
+//
+//
+
+#include <cstdio>
+#include <cstring>
+#include <stdint.h>
+
+#include <PacketHeaders.h>
+
+#include "AvatarData.h"
+
+int packFloatAngleToTwoByte(unsigned char* buffer, float angle) {
+    const float ANGLE_CONVERSION_RATIO = (std::numeric_limits<uint16_t>::max() / 360.0);
+    
+    uint16_t angleHolder = floorf((angle + 180) * ANGLE_CONVERSION_RATIO);
+    memcpy(buffer, &angleHolder, sizeof(uint16_t));
+    
+    return sizeof(uint16_t);
+}
+
+int unpackFloatAngleFromTwoByte(uint16_t *byteAnglePointer, float *destinationPointer) {
+    *destinationPointer = (*byteAnglePointer / std::numeric_limits<uint16_t>::max()) * 360.0 - 180;
+    return sizeof(uint16_t);
+}
+
+AvatarData::AvatarData() :
+    _bodyYaw(-90.0),
+    _bodyPitch(0.0),
+    _bodyRoll(0.0) {
+    
+}
+
+AvatarData::~AvatarData() {
+    
+}
+
+AvatarData* AvatarData::clone() const {
+    return new AvatarData(*this);
+}
+
+// transmit data to agents requesting it
+// called on me just prior to sending data to others (continuasly called)
+int AvatarData::getBroadcastData(unsigned char* destinationBuffer) {
+    unsigned char* bufferStart = destinationBuffer;
+    
+    // TODO: DRY this up to a shared method
+    // that can pack any type given the number of bytes
+    // and return the number of bytes to push the pointer
+    memcpy(destinationBuffer, &_bodyPosition, sizeof(float) * 3);
+    destinationBuffer += sizeof(float) * 3;
+    
+    destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _bodyYaw);
+    destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _bodyPitch);
+    destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _bodyRoll);
+    
+    return destinationBuffer - bufferStart;
+}
+
+// called on the other agents - assigns it to my views of the others
+void AvatarData::parseData(unsigned char* sourceBuffer, int numBytes) {
+    
+    // increment to push past the packet header
+    sourceBuffer++;
+    
+    memcpy(&_bodyPosition, sourceBuffer, sizeof(float) * 3);
+    sourceBuffer += sizeof(float) * 3;
+   
+    sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_bodyYaw);
+    sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_bodyPitch);
+    sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_bodyRoll);
+}
+
+glm::vec3 AvatarData::getBodyPosition() {
+    return glm::vec3(_bodyPosition.x,
+                     _bodyPosition.y,
+                     _bodyPosition.z);
+}
+
+void AvatarData::setBodyPosition(glm::vec3 bodyPosition) {
+    _bodyPosition = bodyPosition;
+}
+
+float AvatarData::getBodyYaw() {
+    return _bodyYaw;
+}
+
+void AvatarData::setBodyYaw(float bodyYaw) {
+    _bodyYaw = bodyYaw;
+}
+
+float AvatarData::getBodyPitch() {
+    return _bodyPitch;
+}
+
+void AvatarData::setBodyPitch(float bodyPitch) {
+    _bodyPitch = bodyPitch;
+}
+
+float AvatarData::getBodyRoll() {
+    return _bodyRoll;
+}
+
+void AvatarData::setBodyRoll(float bodyRoll) {
+    _bodyRoll = bodyRoll;
+}
+
+

libraries/avatars/src/AvatarData.h

@@ -0,0 +1,48 @@
+//
+//  AvatarData.h
+//  hifi
+//
+//  Created by Stephen Birarda on 4/9/13.
+//
+//
+
+#ifndef __hifi__AvatarData__
+#define __hifi__AvatarData__
+
+#include <iostream>
+
+#include <glm/glm.hpp>
+
+#include <AgentData.h>
+
+class AvatarData : public AgentData {
+public:
+    AvatarData();
+    ~AvatarData();
+    
+    AvatarData* clone() const;
+    
+    glm::vec3 getBodyPosition();
+    void setBodyPosition(glm::vec3 bodyPosition);
+    
+    int getBroadcastData(unsigned char* destinationBuffer);
+    void parseData(unsigned char* sourceBuffer, int numBytes);
+    
+    float getBodyYaw();
+    void  setBodyYaw(float bodyYaw);
+    
+    float getBodyPitch();
+    void  setBodyPitch(float bodyPitch);
+    
+    float getBodyRoll();
+    void  setBodyRoll(float bodyRoll);
+    
+protected:
+    glm::vec3 _bodyPosition;
+    
+    float _bodyYaw;
+    float _bodyPitch;
+    float _bodyRoll;
+};
+
+#endif /* defined(__hifi__AvatarData__) */

libraries/avatars/src/Orientation.cpp

@@ -0,0 +1,160 @@
+//-----------------------------------------------------------
+//
+// Created by Jeffrey Ventrella  
+// Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
+//
+//-----------------------------------------------------------
+
+#include "Orientation.h"
+#include <SharedUtil.h>
+
+
+static bool testingForNormalizationAndOrthogonality = true;
+
+Orientation::Orientation() {
+    setToIdentity();
+}
+
+
+void Orientation::setToIdentity() {
+	right	= glm::vec3(  1.0,  0.0,  0.0 );
+	up		= glm::vec3(  0.0,  1.0,  0.0 );
+	front	= glm::vec3(  0.0,  0.0,  1.0 );	
+}
+
+
+void Orientation::set( Orientation o ) { 
+	right	= o.right;
+	up		= o.up;
+	front	= o.front;	
+}
+
+
+void Orientation::yaw( float angle ) {
+	float r = angle * PI_OVER_180;
+	float s = sin(r);
+	float c = cos(r);
+	
+	glm::vec3 cosineFront	= front * c;
+	glm::vec3 cosineRight	= right * c;
+	glm::vec3 sineFront		= front * s;
+	glm::vec3 sineRight		= right * s;
+	
+	front	= cosineFront + sineRight;
+	right	= cosineRight - sineFront;	
+    
+    if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); }
+}
+
+
+void Orientation::pitch( float angle ) {
+	float r = angle * PI_OVER_180;
+	float s = sin(r);
+	float c = cos(r);
+	
+	glm::vec3 cosineUp		= up	* c;
+	glm::vec3 cosineFront	= front	* c;
+	glm::vec3 sineUp		= up	* s;
+	glm::vec3 sineFront		= front * s;
+	
+	up		= cosineUp		+ sineFront;
+	front	= cosineFront	- sineUp;
+
+    if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); }
+}
+
+
+void Orientation::roll( float angle ) {
+	float r = angle * PI_OVER_180;
+	float s = sin(r);
+	float c = cos(r);
+	
+	glm::vec3 cosineUp		= up	* c;
+	glm::vec3 cosineRight	= right	* c;
+	glm::vec3 sineUp		= up	* s;
+	glm::vec3 sineRight		= right	* s;
+	
+	up		= cosineUp		+ sineRight;
+	right	= cosineRight	- sineUp;	
+
+    if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); }
+}
+
+
+void Orientation::setRightUpFront( const glm::vec3 &r, const glm::vec3 &u, const glm::vec3 &f ) {
+	right	= r;
+	up		= u;
+	front	= f;
+}
+
+
+
+//----------------------------------------------------------------------
+void Orientation::testForOrthogonalAndNormalizedVectors( float epsilon ) {
+
+    //------------------------------------------------------------------
+    // make sure vectors are normalized (or close enough to length 1.0)
+    //------------------------------------------------------------------
+	float rightLength	= glm::length( right );
+	float upLength		= glm::length( up    );
+	float frontLength	= glm::length( front );
+	     
+	if (( rightLength > 1.0f + epsilon )
+	||  ( rightLength < 1.0f - epsilon )) { 
+        printf( "Error in Orientation class: right direction length is %f \n", rightLength ); 
+    }
+	assert ( rightLength > 1.0f - epsilon );
+	assert ( rightLength < 1.0f + epsilon );
+
+
+	if (( upLength > 1.0f + epsilon )
+	||  ( upLength < 1.0f - epsilon )) { 
+        printf( "Error in Orientation class: up direction length is %f \n", upLength ); 
+    }
+	assert ( upLength > 1.0f - epsilon );
+	assert ( upLength < 1.0f + epsilon );
+
+
+	if (( frontLength > 1.0f + epsilon )
+	||  ( frontLength < 1.0f - epsilon )) { 
+        printf( "Error in Orientation class: front direction length is %f \n", frontLength ); 
+    }
+	assert ( frontLength > 1.0f - epsilon );
+	assert ( frontLength < 1.0f + epsilon );
+
+
+
+    //----------------------------------------------------------------
+    // make sure vectors are orthoginal (or close enough)
+    //----------------------------------------------------------------
+    glm::vec3 rightCross    = glm::cross( up, front );
+    glm::vec3 upCross       = glm::cross( front, right );
+    glm::vec3 frontCross    = glm::cross( right, up );
+    
+    float rightDiff = glm::length( rightCross - right );
+    float upDiff    = glm::length( upCross - up );
+    float frontDiff = glm::length( frontCross - front );
+
+
+    if ( rightDiff > epsilon ) { 
+        printf( "Error in Orientation class: right direction not orthogonal to up and/or front. " ); 
+        printf( "The tested cross of up and front is off by %f \n", rightDiff ); 
+    }
+	assert ( rightDiff < epsilon );
+    
+    
+    if ( upDiff > epsilon ) { 
+        printf( "Error in Orientation class: up direction not orthogonal to front and/or right. " ); 
+        printf( "The tested cross of front and right is off by %f \n", upDiff ); 
+    }
+	assert ( upDiff < epsilon );
+
+
+    if ( frontDiff > epsilon ) { 
+        printf( "Error in Orientation class: front direction not orthogonal to right and/or up. " ); 
+        printf( "The tested cross of right and up is off by %f \n", frontDiff ); 
+    }
+	assert ( frontDiff < epsilon );
+}
+
+

libraries/avatars/src/Orientation.h

@@ -39,6 +39,9 @@ public:
 	glm::vec3 getFront()	{ return front;	}
 	
 	void setRightUpFront( const glm::vec3 &, const glm::vec3 &, const glm::vec3 & );
+	
+private: 
+	void testForOrthogonalAndNormalizedVectors( float epsilon );
 };
 
 

libraries/shared/src/Agent.cpp

@@ -19,8 +19,6 @@
 #include <arpa/inet.h>
 #endif
 
-Agent::Agent() {}
-
 Agent::Agent(sockaddr *agentPublicSocket, sockaddr *agentLocalSocket, char agentType, uint16_t thisAgentId) {
     publicSocket = new sockaddr;
     memcpy(publicSocket, agentPublicSocket, sizeof(sockaddr));
@@ -37,7 +35,8 @@ Agent::Agent(sockaddr *agentPublicSocket, sockaddr *agentLocalSocket, char agent
     activeSocket = NULL;
     linkedData = NULL;
     
-    pthread_mutex_init(&deleteMutex, NULL);
+    deleteMutex = new pthread_mutex_t;
+    pthread_mutex_init(deleteMutex, NULL);
 }
 
 Agent::Agent(const Agent &otherAgent) {
@@ -67,25 +66,34 @@ Agent::Agent(const Agent &otherAgent) {
         linkedData = NULL;
     }
     
-    pthread_mutex_init(&deleteMutex, NULL);
+    deleteMutex = new pthread_mutex_t;
+    pthread_mutex_init(deleteMutex, NULL);
 }
 
 Agent& Agent::operator=(Agent otherAgent) {
+    std::cout << "Agent swap constructor called on resize?\n";
     swap(*this, otherAgent);
     return *this;
 }
 
 void Agent::swap(Agent &first, Agent &second) {
     using std::swap;
+    
     swap(first.publicSocket, second.publicSocket);
     swap(first.localSocket, second.localSocket);
     swap(first.activeSocket, second.activeSocket);
     swap(first.type, second.type);
     swap(first.linkedData, second.linkedData);
     swap(first.agentId, second.agentId);
+    swap(first.firstRecvTimeUsecs, second.firstRecvTimeUsecs);
+    swap(first.lastRecvTimeUsecs, second.lastRecvTimeUsecs);
+    swap(first.deleteMutex, second.deleteMutex);
 }
 
 Agent::~Agent() {
+    // the deleteMutex isn't destroyed here
+    // that's handled by the agent list silent agent removal thread
+
     delete publicSocket;
     delete localSocket;
     delete linkedData;

libraries/shared/src/Agent.h

@@ -20,16 +20,7 @@
 #endif
 
 class Agent {    
-    void swap(Agent &first, Agent &second);
-    sockaddr *publicSocket, *localSocket, *activeSocket;
-    char type;
-    uint16_t agentId;
-    double firstRecvTimeUsecs;
-    double lastRecvTimeUsecs;
-    AgentData *linkedData;
-    
 public:
-    Agent();
     Agent(sockaddr *agentPublicSocket, sockaddr *agentLocalSocket, char agentType, uint16_t thisAgentId);
     Agent(const Agent &otherAgent);
     ~Agent();
@@ -38,7 +29,7 @@ public:
     
     bool matches(sockaddr *otherPublicSocket, sockaddr *otherLocalSocket, char otherAgentType);
     
-    pthread_mutex_t deleteMutex;
+    pthread_mutex_t *deleteMutex;
     
     char getType() const;
     const char* getTypeName() const;
@@ -60,6 +51,14 @@ public:
     void setLinkedData(AgentData *newData);
 
     friend std::ostream& operator<<(std::ostream& os, const Agent* agent);
+private:
+    void swap(Agent &first, Agent &second);
+    sockaddr *publicSocket, *localSocket, *activeSocket;
+    char type;
+    uint16_t agentId;
+    double firstRecvTimeUsecs;
+    double lastRecvTimeUsecs;
+    AgentData *linkedData;
 };
 
 std::ostream& operator<<(std::ostream& os, const Agent* agent);

libraries/shared/src/AgentData.h

@@ -12,7 +12,7 @@
 class AgentData {
     public:
         virtual ~AgentData() = 0;
-        virtual void parseData(void * data, int size) = 0;
+        virtual void parseData(unsigned char* sourceBuffer, int numBytes) = 0;
         virtual AgentData* clone() const = 0;
 };
 

libraries/shared/src/AgentList.cpp

@@ -81,10 +81,10 @@ unsigned int AgentList::getSocketListenPort() {
     return socketListenPort;
 }
 
-void AgentList::processAgentData(sockaddr *senderAddress, void *packetData, size_t dataBytes) {
+void AgentList::processAgentData(sockaddr *senderAddress, unsigned char *packetData, size_t dataBytes) {
     switch (((char *)packetData)[0]) {
         case PACKET_HEADER_DOMAIN: {
-            updateList((unsigned char *)packetData, dataBytes);
+            updateList(packetData, dataBytes);
             break;
         }
         case PACKET_HEADER_PING: {
@@ -98,7 +98,7 @@ void AgentList::processAgentData(sockaddr *senderAddress, void *packetData, size
     }
 }
 
-void AgentList::processBulkAgentData(sockaddr *senderAddress, void *packetData, int numTotalBytes, int numBytesPerAgent) {
+void AgentList::processBulkAgentData(sockaddr *senderAddress, unsigned char *packetData, int numTotalBytes, int numBytesPerAgent) {
     // find the avatar mixer in our agent list and update the lastRecvTime from it
     int bulkSendAgentIndex = indexOfMatchingAgent(senderAddress);
 
@@ -120,6 +120,7 @@ void AgentList::processBulkAgentData(sockaddr *senderAddress, void *packetData,
         memcpy(packetHolder + 1, currentPosition, numBytesPerAgent);
         
         int matchingAgentIndex = indexOfMatchingAgent(agentID);
+        
         if (matchingAgentIndex >= 0) {
             updateAgentWithData(&agents[matchingAgentIndex], packetHolder, numBytesPerAgent + 1);
         }
@@ -130,7 +131,7 @@ void AgentList::processBulkAgentData(sockaddr *senderAddress, void *packetData,
     delete[] packetHolder;
 }
 
-void AgentList::updateAgentWithData(sockaddr *senderAddress, void *packetData, size_t dataBytes) {
+void AgentList::updateAgentWithData(sockaddr *senderAddress, unsigned char *packetData, size_t dataBytes) {
     // find the agent by the sockaddr
     int agentIndex = indexOfMatchingAgent(senderAddress);
     
@@ -139,7 +140,7 @@ void AgentList::updateAgentWithData(sockaddr *senderAddress, void *packetData, s
     }
 }
 
-void AgentList::updateAgentWithData(Agent *agent, void *packetData, int dataBytes) {
+void AgentList::updateAgentWithData(Agent *agent, unsigned char *packetData, int dataBytes) {
     agent->setLastRecvTimeUsecs(usecTimestampNow());
     
     if (agent->getLinkedData() == NULL) {
@@ -257,7 +258,7 @@ bool AgentList::addOrUpdateAgent(sockaddr *publicSocket, sockaddr *localSocket,
     }    
 }
 
-void AgentList::broadcastToAgents(char *broadcastData, size_t dataBytes, const char* agentTypes, int numAgentTypes) {
+void AgentList::broadcastToAgents(unsigned char *broadcastData, size_t dataBytes, const char* agentTypes, int numAgentTypes) {
     for(std::vector<Agent>::iterator agent = agents.begin(); agent != agents.end(); agent++) {
         // only send to the AgentTypes we are asked to send to.
         if (agent->getActiveSocket() != NULL && memchr(agentTypes, agent->getType(), numAgentTypes)) {
@@ -330,7 +331,7 @@ void *removeSilentAgents(void *args) {
         
         for(std::vector<Agent>::iterator agent = agents->begin(); agent != agents->end();) {
             
-            pthread_mutex_t * agentDeleteMutex = &agent->deleteMutex;
+            pthread_mutex_t* agentDeleteMutex = agent->deleteMutex;
             
             if ((checkTimeUSecs - agent->getLastRecvTimeUsecs()) > AGENT_SILENCE_THRESHOLD_USECS 
             	&& agent->getType() != AGENT_TYPE_VOXEL

libraries/shared/src/AgentList.h

@@ -49,13 +49,13 @@ public:
     
     bool addOrUpdateAgent(sockaddr *publicSocket, sockaddr *localSocket, char agentType, uint16_t agentId);
     
-    void processAgentData(sockaddr *senderAddress, void *packetData, size_t dataBytes);
+    void processAgentData(sockaddr *senderAddress, unsigned char *packetData, size_t dataBytes);
-    void processBulkAgentData(sockaddr *senderAddress, void *packetData, int numTotalBytes, int numBytesPerAgent);
+    void processBulkAgentData(sockaddr *senderAddress, unsigned char *packetData, int numTotalBytes, int numBytesPerAgent);
    
-    void updateAgentWithData(sockaddr *senderAddress, void *packetData, size_t dataBytes);
+    void updateAgentWithData(sockaddr *senderAddress, unsigned char *packetData, size_t dataBytes);
-    void updateAgentWithData(Agent *agent, void *packetData, int dataBytes);
+    void updateAgentWithData(Agent *agent, unsigned char *packetData, int dataBytes);
     
-    void broadcastToAgents(char *broadcastData, size_t dataBytes, const char* agentTypes, int numAgentTypes);
+    void broadcastToAgents(unsigned char *broadcastData, size_t dataBytes, const char* agentTypes, int numAgentTypes);
     char getOwnerType();
     unsigned int getSocketListenPort();
     

libraries/shared/src/AudioRingBuffer.cpp

@@ -105,12 +105,10 @@ void AudioRingBuffer::setBearing(float newBearing) {
     bearing = newBearing;
 }
 
-void AudioRingBuffer::parseData(void *data, int size) {
+void AudioRingBuffer::parseData(unsigned char* sourceBuffer, int numBytes) {    
-    unsigned char *audioDataStart = (unsigned char *) data;
+    if (numBytes > (bufferLengthSamples * sizeof(int16_t))) {
         
-    if (size > (bufferLengthSamples * sizeof(int16_t))) {
+        unsigned char *dataPtr = sourceBuffer + 1;
-        
-        unsigned char *dataPtr = audioDataStart + 1;
         
         for (int p = 0; p < 3; p ++) {
             memcpy(&position[p], dataPtr, sizeof(float));
@@ -123,7 +121,7 @@ void AudioRingBuffer::parseData(void *data, int size) {
         memcpy(&bearing, dataPtr, sizeof(float));
         dataPtr += sizeof(float);
         
-        audioDataStart = dataPtr;
+        sourceBuffer = dataPtr;
     }
 
     if (endOfLastWrite == NULL) {
@@ -134,7 +132,7 @@ void AudioRingBuffer::parseData(void *data, int size) {
         started = false;
     }
     
-    memcpy(endOfLastWrite, audioDataStart, bufferLengthSamples * sizeof(int16_t));
+    memcpy(endOfLastWrite, sourceBuffer, bufferLengthSamples * sizeof(int16_t));
     
     endOfLastWrite += bufferLengthSamples;
     

libraries/shared/src/AudioRingBuffer.h

@@ -19,7 +19,7 @@ class AudioRingBuffer : public AgentData {
         ~AudioRingBuffer();
         AudioRingBuffer(const AudioRingBuffer &otherRingBuffer);
     
-        void parseData(void *data, int size);
+        void parseData(unsigned char* sourceBuffer, int numBytes);
         AudioRingBuffer* clone() const;
 
         int16_t* getNextOutput();

libraries/shared/src/SharedUtil.cpp

@@ -74,7 +74,7 @@ bool oneAtBit(unsigned char byte, int bitIndex) {
     return (byte >> (7 - bitIndex) & 1);
 }
 
-void switchToResourcesIfRequired() {
+void switchToResourcesParentIfRequired() {
 #ifdef __APPLE__
     CFBundleRef mainBundle = CFBundleGetMainBundle();
     CFURLRef resourcesURL = CFBundleCopyResourcesDirectoryURL(mainBundle);
@@ -85,6 +85,7 @@ void switchToResourcesIfRequired() {
     CFRelease(resourcesURL);
     
     chdir(path);
+    chdir("..");
 #endif
 }
 

libraries/shared/src/SharedUtil.h

@@ -10,6 +10,7 @@
 #define __hifi__SharedUtil__
 
 #include <stdint.h>
+#include <math.h>
 
 #ifdef _WIN32
 #include "Systime.h"
@@ -17,6 +18,21 @@
 #include <sys/time.h>
 #endif
 
+static const float ZERO             = 0.0f;
+static const float ONE              = 1.0f;
+static const float ONE_HALF			= 0.5f;
+static const float ONE_THIRD        = 0.333333f;
+static const float PIE              = 3.141592f;
+static const float PI_TIMES_TWO		= 3.141592f * 2.0f;
+static const float PI_OVER_180      = 3.141592f / 180.0f;
+static const float EPSILON          = 0.000001f;	//smallish positive number - used as margin of error for some computations
+static const float SQUARE_ROOT_OF_2 = (float)sqrt(2);
+static const float SQUARE_ROOT_OF_3 = (float)sqrt(3);
+static const float METER            = 1.0f;
+static const float DECIMETER        = 0.1f;
+static const float CENTIMETER       = 0.01f;
+static const float MILLIIMETER      = 0.001f;
+
 double usecTimestamp(timeval *time);
 double usecTimestampNow();
 
@@ -31,7 +47,7 @@ void printVoxelCode(unsigned char* voxelCode);
 int numberOfOnes(unsigned char byte);
 bool oneAtBit(unsigned char byte, int bitIndex);
 
-void switchToResourcesIfRequired();
+void switchToResourcesParentIfRequired();
 
 const char* getCmdOption(int argc, const char * argv[],const char* option);
 bool cmdOptionExists(int argc, const char * argv[],const char* option);

libraries/voxels/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8)
 
-set(ROOT_DIR ../../)
+set(ROOT_DIR ../..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
 # setup for find modules
@@ -8,19 +8,11 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../../cm
 
 set(TARGET_NAME voxels)
 
-project(${TARGET_NAME})
+include(${MACRO_DIR}/SetupHifiLibrary.cmake)
+setup_hifi_library(${TARGET_NAME})
 
-# set up the external glm library
 include(${MACRO_DIR}/IncludeGLM.cmake)
-include_glm(${TARGET_NAME} ${MACRO_DIR})
+include_glm(${TARGET_NAME} ${ROOT_DIR})
-
-# grab the implemenation and header files
-file(GLOB HIFI_VOXELS_SRCS src/*.h src/*.cpp)
-
-# create a library and set the property so it can be referenced later
-add_library(${TARGET_NAME} ${HIFI_VOXELS_SRCS})
 
 include(${MACRO_DIR}/LinkHifiLibrary.cmake)
 link_hifi_library(shared ${TARGET_NAME} ${ROOT_DIR})
-
-set(HIFI_VOXELS_LIBRARY ${TARGET_NAME})

libraries/voxels/src/AABox.cpp

@@ -0,0 +1,76 @@
+/* ------------------------------------------------------
+
+ Axis Aligned Boxes - Lighthouse3D
+
+  -----------------------------------------------------*/
+
+#include "AABox.h"
+
+
+AABox::AABox(const glm::vec3& corner,  float x, float y, float z) {
+	setBox(corner,x,y,z);
+}
+
+AABox::AABox(void) {
+	corner.x = 0; corner.y = 0; corner.z = 0;
+	x = 1.0f;
+	y = 1.0f;
+	z = 1.0f;
+}
+
+
+AABox::~AABox() {}
+
+	
+
+void AABox::setBox(const glm::vec3& corner,  float x, float y, float z) {
+	this->corner = corner;
+	if (x < 0.0) {
+		x = -x;
+		this->corner.x -= x;
+	}
+	if (y < 0.0) {
+		y = -y;
+		this->corner.y -= y;
+	}
+	if (z < 0.0) {
+		z = -z;
+		this->corner.z -= z;
+	}
+	this->x = x;
+	this->y = y;
+	this->z = z;
+}
+
+
+
+glm::vec3 AABox::getVertexP(const glm::vec3 &normal) {
+	glm::vec3 res = corner;
+	if (normal.x > 0)
+		res.x += x;
+
+	if (normal.y > 0)
+		res.y += y;
+
+	if (normal.z > 0)
+		res.z += z;
+
+	return(res);
+}
+
+
+
+glm::vec3 AABox::getVertexN(const glm::vec3 &normal) {
+	glm::vec3 res = corner;
+
+	if (normal.x < 0)
+		res.x += x;
+
+	if (normal.y < 0)
+		res.y += y;
+
+	if (normal.z < 0)
+		res.z += z;
+
+	return(res);
+}

libraries/voxels/src/AABox.h

@@ -0,0 +1,33 @@
+/* ------------------------------------------------------
+
+ Axis Aligned Boxes - Lighthouse3D
+
+  -----------------------------------------------------*/
+
+
+#ifndef _AABOX_
+#define _AABOX_
+
+#include <glm/glm.hpp>
+
+class AABox 
+{
+
+public:
+
+	glm::vec3 corner;
+	float x,y,z;
+
+	AABox(const glm::vec3 &corner, float x, float y, float z);
+	AABox(void);
+	~AABox();
+
+	void setBox(const glm::vec3& corner, float x, float y, float z);
+
+	// for use in frustum computations
+	glm::vec3 getVertexP(const glm::vec3& normal);
+	glm::vec3 getVertexN(const glm::vec3& normal);
+};
+
+
+#endif

libraries/voxels/src/Plane.cpp

@@ -0,0 +1,83 @@
+// Plane.cpp
+//
+//////////////////////////////////////////////////////////////////////
+
+#include "Plane.h"
+#include <stdio.h>
+
+// These are some useful utilities that vec3 is missing
+float vec3_length(const glm::vec3& v) {
+	return((float)sqrt(v.x*v.x + v.y*v.y + v.z*v.z));
+}
+
+void vec3_normalize(glm::vec3& v) {
+
+	float len;
+
+	len = vec3_length(v);
+	if (len) {
+		v.x /= len;;
+		v.y /= len;
+		v.z /= len;
+	}
+}
+
+float vec3_innerProduct(const glm::vec3& v1,const glm::vec3& v2) {
+    
+	return (v1.x * v2.x + v1.y * v2.y + v1.z * v2.z);
+}
+
+
+
+Plane::Plane(const glm::vec3 &v1, const glm::vec3 &v2, const glm::vec3 &v3) {
+
+	set3Points(v1,v2,v3);
+}
+
+
+Plane::Plane() {}
+
+Plane::~Plane() {}
+
+
+void Plane::set3Points(const glm::vec3 &v1, const glm::vec3 &v2, const glm::vec3 &v3) {
+
+
+	glm::vec3 aux1, aux2;
+
+	aux1 = v1 - v2;
+	aux2 = v3 - v2;
+
+	normal = aux2 * aux1;
+
+	vec3_normalize(normal);
+	point = v2;
+	d = -(vec3_innerProduct(normal,point));
+}
+
+void Plane::setNormalAndPoint(const glm::vec3 &normal, const glm::vec3 &point) {
+
+	this->normal = normal;
+	vec3_normalize(this->normal);
+	d = -(vec3_innerProduct(this->normal,point));
+}
+
+void Plane::setCoefficients(float a, float b, float c, float d) {
+
+	// set the normal vector
+	normal = glm::vec3(a,b,c);
+	//compute the lenght of the vector
+	float l = normal.length();
+	// normalize the vector
+	normal = glm::vec3(a/l,b/l,c/l);
+	// and divide d by th length as well
+	this->d = d/l;
+}
+
+float Plane::distance(const glm::vec3 &p) {
+    return (d + vec3_innerProduct(normal,p));
+}
+
+void Plane::print() {
+	//printf("Plane(");normal.print();printf("# %f)",d);
+}

libraries/voxels/src/Plane.h

@@ -0,0 +1,35 @@
+//////////////////////////////////////////////////////////////////////
+// Plane.h - inspired and modified from lighthouse3d.com
+//
+
+
+#ifndef _PLANE_
+#define _PLANE_
+
+#include <glm/glm.hpp>
+
+
+class Plane  
+{
+
+public:
+
+	glm::vec3 normal,point;
+	float d;
+
+
+	Plane(const glm::vec3 &v1, const glm::vec3 &v2, const glm::vec3 &v3);
+	Plane(void);
+	~Plane();
+
+	void set3Points(const glm::vec3 &v1, const glm::vec3 &v2, const glm::vec3 &v3);
+	void setNormalAndPoint(const glm::vec3 &normal, const glm::vec3 &point);
+	void setCoefficients(float a, float b, float c, float d);
+	float distance(const glm::vec3 &p);
+
+	void print();
+
+};
+
+
+#endif

libraries/voxels/src/ViewFrustum.cpp

@@ -10,12 +10,29 @@
 
 #include "ViewFrustum.h"
 
-float ViewFrustum::fovAngleAdust=1.65;
+ViewFrustum::ViewFrustum() :
-
+    _position(glm::vec3(0,0,0)),
-ViewFrustum::ViewFrustum(glm::vec3 position, glm::vec3 direction, 
+    _direction(glm::vec3(0,0,0)),
-		glm::vec3 up, glm::vec3 right, float screenWidth, float screenHeight) {
+    _up(glm::vec3(0,0,0)),
-	this->calculateViewFrustum(position, direction, up, right, screenWidth, screenHeight);
+    _right(glm::vec3(0,0,0)),
-}
+    _fieldOfView(0.0),
+    _aspectRatio(1.0),
+    _nearClip(0.1),
+    _farClip(500.0),
+    _nearHeight(0.0),
+    _nearWidth(0.0),
+    _farHeight(0.0),
+    _farWidth(0.0),
+    _farCenter(glm::vec3(0,0,0)),
+    _farTopLeft(glm::vec3(0,0,0)),
+    _farTopRight(glm::vec3(0,0,0)),
+    _farBottomLeft(glm::vec3(0,0,0)),
+    _farBottomRight(glm::vec3(0,0,0)),
+    _nearCenter(glm::vec3(0,0,0)),
+    _nearTopLeft(glm::vec3(0,0,0)),
+    _nearTopRight(glm::vec3(0,0,0)),
+    _nearBottomLeft(glm::vec3(0,0,0)),
+    _nearBottomRight(glm::vec3(0,0,0)) { }
 
 /////////////////////////////////////////////////////////////////////////////////////
 // ViewFrustum::calculateViewFrustum()
@@ -26,51 +43,54 @@ ViewFrustum::ViewFrustum(glm::vec3 position, glm::vec3 direction,
 // Notes on how/why this works: 
 //     http://www.lighthouse3d.com/tutorials/view-frustum-culling/view-frustums-shape/
 //
-void ViewFrustum::calculateViewFrustum(glm::vec3 position, glm::vec3 direction, 
+void ViewFrustum::calculate() {
-		glm::vec3 up, glm::vec3 right, float screenWidth, float screenHeight) {
 
-	// Save the values we were passed...
+    static const double PI_OVER_180			= 3.14159265359 / 180.0; // would be better if this was in a shared location
-	this->_position = position;
-	this->_direction = direction;
-	this->_up = up;
-	this->_right = right;
-	this->_screenWidth = screenWidth;
-	this->_screenHeight = screenHeight;
 	
-	glm::vec3 front    = direction;
+	glm::vec3 front    = _direction;
 	
 	// Calculating field of view.
-	//    0.7854f is 45 deg
+	float fovInRadians = this->_fieldOfView * PI_OVER_180;
-	//    ViewFrustum::fovAngleAdust defaults to 1.65
-	// Apparently our fov is around 1.75 times this value or 74.25 degrees
-	// you can adjust this in interface by using the "|" and "\" keys to tweak
-	// the adjustment and see effects of different FOVs
-	float fovHalfAngle = 0.7854f * ViewFrustum::fovAngleAdust; 
-	float ratio        = screenWidth / screenHeight;
 	
-	this->_nearDist = 0.1;
+	float twoTimesTanHalfFOV = 2.0f * tan(fovInRadians/2.0f);
-	this->_farDist  = 10.0;
 
-	this->_nearHeight = 2 * tan(fovHalfAngle) * this->_nearDist;
+    // Do we need this?
-	this->_nearWidth  = this->_nearHeight * ratio;
+	//tang = (float)tan(ANG2RAD * angle * 0.5) ;
-	this->_farHeight  = 2 * tan(fovHalfAngle) * this->_farDist;
-	this->_farWidth   = this->_farHeight * ratio;
 
-	float farHalfHeight    = this->_farHeight * 0.5f;
+    float nearClip = this->_nearClip;
-	float farHalfWidth     = this->_farWidth  * 0.5f;
+    float farClip  = this->_farClip;
-	this->_farCenter       = this->_position+front * this->_farDist;
+	
+	this->_nearHeight = (twoTimesTanHalfFOV * nearClip);
+	this->_nearWidth  = this->_nearHeight * this->_aspectRatio;
+	this->_farHeight  = (twoTimesTanHalfFOV * farClip);
+	this->_farWidth   = this->_farHeight * this->_aspectRatio;
+
+	float farHalfHeight    = (this->_farHeight * 0.5f);
+	float farHalfWidth     = (this->_farWidth  * 0.5f);
+	this->_farCenter       = this->_position+front * farClip;
 	this->_farTopLeft      = this->_farCenter  + (this->_up * farHalfHeight)  - (this->_right * farHalfWidth); 
 	this->_farTopRight     = this->_farCenter  + (this->_up * farHalfHeight)  + (this->_right * farHalfWidth); 
 	this->_farBottomLeft   = this->_farCenter  - (this->_up * farHalfHeight)  - (this->_right * farHalfWidth); 
 	this->_farBottomRight  = this->_farCenter  - (this->_up * farHalfHeight)  + (this->_right * farHalfWidth); 
 
-	float nearHalfHeight   = this->_nearHeight * 0.5f;
+	float nearHalfHeight   = (this->_nearHeight * 0.5f);
-	float nearHalfWidth    = this->_nearWidth  * 0.5f;
+	float nearHalfWidth    = (this->_nearWidth  * 0.5f);
-	this->_nearCenter      = this->_position+front * this->_nearDist;
+	this->_nearCenter      = this->_position+front * nearClip;
 	this->_nearTopLeft     = this->_nearCenter + (this->_up * nearHalfHeight) - (this->_right * nearHalfWidth); 
 	this->_nearTopRight    = this->_nearCenter + (this->_up * nearHalfHeight) + (this->_right * nearHalfWidth); 
 	this->_nearBottomLeft  = this->_nearCenter - (this->_up * nearHalfHeight) - (this->_right * nearHalfWidth); 
 	this->_nearBottomRight = this->_nearCenter - (this->_up * nearHalfHeight) + (this->_right * nearHalfWidth); 
+
+	// compute the six planes
+	// the function set3Points assumes that the points
+	// are given in counter clockwise order
+	this->_planes[TOPP].set3Points(this->_nearTopRight,this->_nearTopLeft,this->_farTopLeft);
+	this->_planes[BOTTOMP].set3Points(this->_nearBottomLeft,this->_nearBottomRight,this->_farBottomRight);
+	this->_planes[LEFTP].set3Points(this->_nearTopLeft,this->_nearBottomLeft,this->_farBottomLeft);
+	this->_planes[RIGHTP].set3Points(this->_nearBottomRight,this->_nearTopRight,this->_farBottomRight);
+	this->_planes[NEARP].set3Points(this->_nearTopLeft,this->_nearTopRight,this->_nearBottomRight);
+	this->_planes[FARP].set3Points(this->_farTopRight,this->_farTopLeft,this->_farBottomLeft);
+
 }
 
 void ViewFrustum::dump() {
@@ -80,11 +100,11 @@ void ViewFrustum::dump() {
     printf("up.x=%f, up.y=%f, up.z=%f\n", this->_up.x, this->_up.y, this->_up.z);
     printf("right.x=%f, right.y=%f, right.z=%f\n", this->_right.x, this->_right.y, this->_right.z);
 
-    printf("farDist=%f\n", this->_farDist);
+    printf("farDist=%f\n", this->_farClip);
     printf("farHeight=%f\n", this->_farHeight);
     printf("farWidth=%f\n", this->_farWidth);
 
-    printf("nearDist=%f\n", this->_nearDist);
+    printf("nearDist=%f\n", this->_nearClip);
     printf("nearHeight=%f\n", this->_nearHeight);
     printf("nearWidth=%f\n", this->_nearWidth);
 
@@ -112,3 +132,37 @@ void ViewFrustum::dump() {
 }
 
 
+int ViewFrustum::pointInFrustum(glm::vec3 &p) {
+	int result = INSIDE;
+	for(int i=0; i < 6; i++) {
+		if (this->_planes[i].distance(p) < 0)
+			return OUTSIDE;
+	}
+	return(result);
+}
+
+int ViewFrustum::sphereInFrustum(glm::vec3 &center, float radius) {
+	int result = INSIDE;
+	float distance;
+	for(int i=0; i < 6; i++) {
+		distance = this->_planes[i].distance(center);
+		if (distance < -radius)
+			return OUTSIDE;
+		else if (distance < radius)
+			result =  INTERSECT;
+	}
+	return(result);
+}
+
+
+int ViewFrustum::boxInFrustum(AABox &b) {
+	int result = INSIDE;
+	for(int i=0; i < 6; i++) {
+		if (this->_planes[i].distance(b.getVertexP(this->_planes[i].normal)) < 0)
+			return OUTSIDE;
+		else if (this->_planes[i].distance(b.getVertexN(this->_planes[i].normal)) < 0)
+			result =  INTERSECT;
+	}
+	return(result);
+ }
+

libraries/voxels/src/ViewFrustum.h

@@ -12,36 +12,60 @@
 #define __hifi__ViewFrustum__
 
 #include <glm/glm.hpp>
+#include "Plane.h"
+#include "AABox.h"
 
 class ViewFrustum {
 private:
+
+    // camera location/orientation attributes
     glm::vec3   _position;
     glm::vec3   _direction;
     glm::vec3   _up;
     glm::vec3   _right;
-	float _screenWidth;
-	float _screenHeight;
 
-	float _nearDist; 
+    // Lens attributes
-	float _farDist;
+    float       _fieldOfView;
+    float       _aspectRatio;
+    float       _nearClip; 
+    float       _farClip;
 
+    // Calculated values
     float       _nearHeight;
     float       _nearWidth;
     float       _farHeight;
     float       _farWidth;
-
     glm::vec3   _farCenter;
     glm::vec3   _farTopLeft;      
     glm::vec3   _farTopRight;     
     glm::vec3   _farBottomLeft;   
     glm::vec3   _farBottomRight;  
-
     glm::vec3   _nearCenter; 
     glm::vec3   _nearTopLeft;     
     glm::vec3   _nearTopRight;    
     glm::vec3   _nearBottomLeft;  
     glm::vec3   _nearBottomRight;
+    enum { TOPP = 0, BOTTOMP, LEFTP, RIGHTP, NEARP, FARP };
+    Plane _planes[6]; // How will this be used?
+    
 public:
+    // setters for camera attributes
+    void setPosition        (const glm::vec3& p) { _position = p; }
+    void setOrientation     (const glm::vec3& d, const glm::vec3& u, const glm::vec3& r ) 
+        { _direction = d; _up = u; _right = r; }
+
+    // setters for lens attributes
+    void setFieldOfView     ( float f ) { _fieldOfView      = f; }
+    void setAspectRatio     ( float a ) { _aspectRatio      = a; }
+    void setNearClip        ( float n ) { _nearClip         = n; }
+    void setFarClip         ( float f ) { _farClip          = f; }
+
+    // getters for lens attributes
+    float getFieldOfView()                  const { return _fieldOfView;    };
+    float getAspectRatio()                  const { return _aspectRatio;    };
+    float getNearClip()                     const { return _nearClip;       };
+    float getFarClip()                      const { return _farClip;        };
+
     const glm::vec3& getFarCenter()         const { return _farCenter;      };
     const glm::vec3& getFarTopLeft()        const { return _farTopLeft;     };  
     const glm::vec3& getFarTopRight()       const { return _farTopRight;    };
@@ -52,17 +76,20 @@ public:
     const glm::vec3& getNearTopLeft()       const { return _nearTopLeft;    };  
     const glm::vec3& getNearTopRight()      const { return _nearTopRight;   };
     const glm::vec3& getNearBottomLeft()    const { return _nearBottomLeft; };
-	const glm::vec3& getNearBottomRight() const { return _nearBottomRight; };
+    const glm::vec3& getNearBottomRight()   const { return _nearBottomRight;};
 
-	void calculateViewFrustum(glm::vec3 position, glm::vec3 direction, 
+    void calculate();
-		glm::vec3 up, glm::vec3 right, float screenWidth, float screenHeight);
 
-	ViewFrustum(glm::vec3 position, glm::vec3 direction, 
+    ViewFrustum();
-		glm::vec3 up, glm::vec3 right, float screenWidth, float screenHeight);
 
     void dump();
     
-	static float fovAngleAdust;
+    enum {OUTSIDE, INTERSECT, INSIDE};
+
+    int pointInFrustum(glm::vec3 &p);
+    int sphereInFrustum(glm::vec3 &center, float radius);
+    int boxInFrustum(AABox &b);
+    
 };
 
 

libraries/voxels/src/VoxelTree.cpp

@@ -192,37 +192,23 @@ void VoxelTree::deleteVoxelCodeFromTree(unsigned char *codeBuffer) {
 	VoxelNode* parentNode = NULL;
     VoxelNode* nodeToDelete = nodeForOctalCode(rootNode, codeBuffer, &parentNode);
     
-    printf("deleteVoxelCodeFromTree() looking [codeBuffer] for:\n");
-    printOctalCode(codeBuffer);
-
-    printf("deleteVoxelCodeFromTree() found [nodeToDelete->octalCode] for:\n");
-    printOctalCode(nodeToDelete->octalCode);
-    
     // If the node exists...
 	int lengthInBytes = bytesRequiredForCodeLength(*codeBuffer); // includes octet count, not color!
-	printf("compare octal codes of length %d\n",lengthInBytes);
 
-    if (0==memcmp(nodeToDelete->octalCode,codeBuffer,lengthInBytes)) {
+    if (0 == memcmp(nodeToDelete->octalCode,codeBuffer,lengthInBytes)) {
-    	printf("found node to delete...\n");
 
 		float* vertices = firstVertexForCode(nodeToDelete->octalCode);
-		printf("deleting voxel at: %f,%f,%f\n",vertices[0],vertices[1],vertices[2]);
 		delete []vertices;
 
 		if (parentNode) {
 			float* vertices = firstVertexForCode(parentNode->octalCode);
-			printf("parent of deleting voxel at: %f,%f,%f\n",vertices[0],vertices[1],vertices[2]);
 			delete []vertices;
 			
 			int childNDX = branchIndexWithDescendant(parentNode->octalCode, codeBuffer);
-			printf("child INDEX=%d\n",childNDX);
 
-			printf("deleting Node at parentNode->children[%d]\n",childNDX);
 			delete parentNode->children[childNDX]; // delete the child nodes
-			printf("setting parentNode->children[%d] to NULL\n",childNDX);
 			parentNode->children[childNDX]=NULL; // set it to NULL
 
-			printf("reaverageVoxelColors()\n");
 			reaverageVoxelColors(rootNode); // Fix our colors!! Need to call it on rootNode
 		}
     }

space-server/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8)
 
-set(ROOT_DIR ../)
+set(ROOT_DIR ..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
 set(TARGET_NAME space-server)

voxel-server/CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8)
 
 set(TARGET_NAME voxel-server)
 
-set(ROOT_DIR ../)
+set(ROOT_DIR ..)
 set(MACRO_DIR ${ROOT_DIR}/cmake/macros)
 
 include(${MACRO_DIR}/SetupHifiProject.cmake)

voxel-server/src/VoxelAgentData.cpp

@@ -27,9 +27,10 @@ VoxelAgentData* VoxelAgentData::clone() const {
     return new VoxelAgentData(*this);
 }
 
-void VoxelAgentData::parseData(void *data, int size) {
+void VoxelAgentData::parseData(unsigned char* sourceBuffer, int numBytes) {
+    // push past the packet header
+    sourceBuffer++;
+    
     // pull the position from the interface agent data packet
-    sscanf((char *)data,
+    memcpy(&position, sourceBuffer, sizeof(float) * 3);
-           "H%*f,%*f,%*f,%f,%f,%f",
-           &position[0], &position[1], &position[2]);
 }

voxel-server/src/VoxelAgentData.h

@@ -22,7 +22,7 @@ public:
     ~VoxelAgentData();
     VoxelAgentData(const VoxelAgentData &otherAgentData);
     
-    void parseData(void *data, int size);
+    void parseData(unsigned char* sourceBuffer, int numBytes);
     VoxelAgentData* clone() const;
 };
 

voxel-server/src/main.cpp

@@ -129,7 +129,7 @@ void eraseVoxelTreeAndCleanupAgentVisitData() {
 
 		// 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);
+		pthread_mutex_lock(thisAgent->deleteMutex);
 
 		// clean up the agent visit data
 		delete agentData->rootMarkerNode;
@@ -137,7 +137,7 @@ void eraseVoxelTreeAndCleanupAgentVisitData() {
 		
 		// unlock the delete mutex so the other thread can
 		// kill the agent if it has dissapeared
-		pthread_mutex_unlock(&thisAgent->deleteMutex);
+		pthread_mutex_unlock(thisAgent->deleteMutex);
 	}
 }
 
@@ -167,7 +167,7 @@ void *distributeVoxelsToListeners(void *args) {
             
             // 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);
+            pthread_mutex_lock(thisAgent->deleteMutex);
             
             stopOctal = NULL;
             packetCount = 0;
@@ -207,7 +207,7 @@ void *distributeVoxelsToListeners(void *args) {
             
             // unlock the delete mutex so the other thread can
             // kill the agent if it has dissapeared
-            pthread_mutex_unlock(&thisAgent->deleteMutex);
+            pthread_mutex_unlock(thisAgent->deleteMutex);
         }
         
         // dynamically sleep until we need to fire off the next set of voxels
@@ -289,7 +289,7 @@ int main(int argc, const char * argv[])
     
     sockaddr agentPublicAddress;
     
-    char *packetData = new char[MAX_PACKET_SIZE];
+    unsigned char *packetData = new unsigned char[MAX_PACKET_SIZE];
     ssize_t receivedBytes;
 
     // loop to send to agents requesting data
@@ -352,7 +352,7 @@ int main(int argc, const char * argv[])
 
             	// 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
+				char* command = (char*) &packetData[1]; // start of the command
 				int commandLength = strlen(command); // commands are null terminated strings
                 int totalLength = 1+commandLength+1;
 
@@ -385,7 +385,7 @@ int main(int argc, const char * argv[])
                     agentList->increaseAgentId();
                 }
                 
-                agentList->updateAgentWithData(&agentPublicAddress, (void *)packetData, receivedBytes);
+                agentList->updateAgentWithData(&agentPublicAddress, packetData, receivedBytes);
             }
         }
     }