Appendix A: Code

 

A.1. Device Software

 

A more manageable listing (as well as the project files) for the device code are available here.

 

A.1.1 Project.h

// UINT type definition

#ifndef _UINT_DEF_

#define _UINT_DEF_

typedef unsigned int UINT;

#endif  /* _UINT_DEF_ */

 

// BYTE type definition

#ifndef _BYTE_DEF_

#define _BYTE_DEF_

typedef unsigned char BYTE;

#endif   /* _BYTE_DEF_ */

        

// Global Definitions

#define NUM_CHANNELS          6

#define DATA_PACKET_LENGTH   10

 

//RXC ISR variables                                              

unsigned char r_index;        //current string index

unsigned char r_buffer[16];   //input string

unsigned char r_ready;        //flag for receive done

unsigned char r_char;         //current character 

 

//TX empty ISR variables for getting input                                               

unsigned char t_index;                    //current string index

unsigned char t_buffer[DATA_PACKET_LENGTH];     //output string

unsigned char t_ready;                    //flag for transmit done

unsigned char t_char;                     //current character

 

// Data channel vector

UINT data[NUM_CHANNELS];

 

BYTE time0;       // sampling time counter

BYTE time1;             // transmission time counter

 

// ADMUX values for each channel selected

flash BYTE muxvals[ NUM_CHANNELS ] = {    0b01100000, 0b01100001, 0b01100010,

                              0b01100011, 0b01100101, 0b01100110 };

 

// Function protoytes

void Port_Init(void);

void Timer0_Init(void);

void ACD0_Init(void);

void UART_Init(void);                   

 

void send_data(void);

 

void puts_int(void);

void gets_int(void);

 

 

A.1.2 Project.c

#include <Mega32.h> 

#include <stdio.h>      // sprintf amungst other definitions

#include "project.h"

        

/*********************************************************/

//UART character-ready ISR

interrupt [USART_RXC] void uart_rec(void){

 

      r_char=UDR;    //get a char

      //UDR=r_char;    //then print it

     

      //build the input string

      if (r_char != '\r')

            r_buffer[r_index++]=r_char;

      else{

            putchar('\n');                //use putchar to avoid overwrite

            r_buffer[r_index]=0x00;             //zero terminate

            r_ready=1;                    //signal cmd processor

            UCSRB.7=0;                          //stop rec ISR

      }

}

 

/**********************************************************/

//UART xmit-empty ISR

interrupt [USART_DRE] void uart_send(void){

 

      t_char = t_buffer[++t_index];

    if(t_char == 0){

     

            UCSRB.5=0; //kill isr

            t_ready=1; //transmit done

      }

      else 

      UDR = t_char ;     //send the char

} 

 

/**********************************************************/

// Timer 0 compare ISR                                      

interrupt [TIM0_COMP] void timer0_compare(void){

             

      // increment .5ms time variables

      time0++;    

      time1++;          

       

}

 

/**********************************************************/      

//Entry point and task scheduler loop

void main(void){ 

     

      // temp vars

      BYTE i, prev_time;

      UINT temp;

 

      // set timing variables to zero                  

      time0 = 0; 

      time1 = 0;

 

      Port_Init();            // set up port for proper i/o

      Timer0_Init();          // set up timer 0 

      ACD0_Init();            // set up ADC and start first conversion                            

      UART_Init();            // set up UART

     

      PORTD.7 = 0;            // turn LED on

             

      // enable interrupts

      #asm

            sei

      #endasm  

 

      // spin forever

      while(1){

                         

            // 2.5 ms count tick

            if( time0 == 5 ){

           

                  time0 = 0;

                

                  // sample each channel continuously

                  for( i=0; i<NUM_CHANNELS; i++ ){

                 

                        // change channel

                        ADMUX = muxvals[i];

 

                        // start new conversion

                        ADCSRA.6=1;

                       

                        // poll for completion

                        while( ADCSRA.6 == 1 );

                                      

                        // grab data

                        //data[i] += ADCH;

                        temp = (ADCL>>6);

                        temp |= ( (UINT)ADCH ) << 2;

                        data[i] += temp;

                  }

 

            }

             

            // 10 ms count tick, send the data

            if( time1 == 20 ){

           

                  time1 = 0;

           

                  send_data();

                 

                  data[0] = 0;

                  data[1] = 0;

                  data[2] = 0;

                  data[3] = 0;

                  data[4] = 0;

                  data[5] = 0;     

           

            }

 

      }// end while(1)   

}

 

//--------------------------------------------------------

//    SEND_DATA

//--------------------------------------------------------

// send x,y,z data out to UART

void send_data(){ 

 

      // temp variables

      BYTE i, temp;

 

      // divide the data by 4 (>>2)

      for( i=0; i<NUM_CHANNELS; i++ )

            data[i] = data[i]>>2;

     

                             

      // build the data packet

      t_buffer[0] = 0x01;

      t_buffer[1] = (BYTE)(data[0]&0x00FF);     // data[0] lb -- x1

      t_buffer[2] = (BYTE)(data[1]&0x00FF);     // data[1] lb -- y1

      t_buffer[3] = (BYTE)(data[2]&0x00FF);     // data[2] lb -- z1    

     

      t_buffer[4] = (BYTE)(data[3]&0x00FF);     // data[0] lb -- x2

      t_buffer[5] = (BYTE)(data[4]&0x00FF);     // data[1] lb -- y2

      t_buffer[6] = (BYTE)(data[5]&0x00FF);     // data[2] lb -- z2              

     

      temp =  (data[0]>>2)&0b11000000;

      temp |= (data[1]>>4)&0b00110000;

      temp |= (data[2]>>6)&0b00001100;

      temp |= (data[3]>>8)&0b00000011;

      t_buffer[7] = temp;

     

      temp =  (data[4]>>2)&0b11000000;

      temp |= (data[5]>>4)&0b00110000;   

      t_buffer[8] = temp;

     

      t_buffer[9] = 0x00;

 

      // send the data

      puts_int();

}

 

//---------------------------------------------------------

//    ADC0_Init

//---------------------------------------------------------

// Set up ADC with right adjusted values

// Start first conversion

void ACD0_Init(void){

 

      ADMUX =  0b01100000;          // using AVCC as reference

                              //    /w left adjusted values

                              //    looking @ ADC0

      ADCSRA = 0b10000111;          // enable ADC with interrupt on

                              //    auto trigger off, interrupt enable

                                               

      // start first conversion

      ADCSRA.6 = 1;

}

 

//---------------------------------------------------------

//    Port_Init

//---------------------------------------------------------

// Set up I/O PORTS:

// PORTA: input for Analog Signals

void Port_Init(void){

 

      DDRA = 0x00;      // port A as an input

      PORTA = 0xff;     // turn pull-ups on

                 

      // port C,D as digital output

      DDRC = 0xff;

      DDRD = 0xff;

}

 

//---------------------------------------------------------

//    Timer0_Init

//---------------------------------------------------------

// Set up timer 0 with interrupt ticks @ .5ms

void Timer0_Init(void){ 

 

      TCCR0 = 0b00001011;     // clock source @ /64 internal clock

      OCR0 = 125;             // compare value of 125

      TIMSK = 0x02;             // enable compare match

}

 

//---------------------------------------------------------

//    UART0_Init

//---------------------------------------------------------

// Set up Uart @ 9600 baud

void UART_Init(void){

   

      UCSRB = 0x18 ;

      UBRRL = 103 ;

     

      //init the task timers  

      r_ready=0;

      t_ready=1;

 

}

 

//**********************************************************

//  -- non-blocking keyboard check initializes ISR-driven

// receive. This routine merely sets up the ISR, which then

//does all the work of getting a command.

void gets_int(void){

 

      r_ready=0;

      r_index=0;

      UCSRB.7=1;  // enables UART_RXC interrupt

}

 

//**********************************************************

//  -- nonblocking print: initializes ISR-driven

// transmit. This routine merely sets up the ISR, then

//send one character, The ISR does all the work.  

void puts_int(void){

 

      t_ready=0;

      t_index=0;

      if (t_buffer[0]>0){

            putchar(t_buffer[0]);

            UCSRB.5=1;  // enables UART_DRE interrupt

      }

}

 

A.2. Application Software

 

A more manageable listing of the code (including the visual studio project files) are available here.

 

A.2.1: Main.cpp

 

#define GAMEX_MAIN

 

#include <windows.h>

#include <process.h>

#include <stdio.h>

 

#include "gamex.hpp"

#include "LogFile.hpp"

#include "Console.hpp"

#include "Graph.hpp"

#include "Button.hpp"

#include "SerialPort.hpp"

#include "Point.hpp"

#include "Elbow.hpp"

#include "Wrist.hpp"

#include "Body.hpp"

 

#define PI  3.14159f

 

int width = 512;

int   height = 768;

char buffer[128];

 

// Camera for 3D projections

CameraX cam;                                   

Vector3DF view;

Vector3DF axisView;

char txt_x[] = "x";

char txt_y[] = "y";

char txt_z[] = "z";

 

// GUI

HWND hWnd;

Console *console;

Graph *accGraph[3], *velGraph[3], *posGraph[3], *angGraph[3];

bool showAllGraphs = false, graphsDisabled = true;

Button *startLogButton, *saveLogButton, *runLogButton, *resetButton;

 

// Motion Capture

Wrist *wrist;

Elbow *elbow;

Point *shoulder;

Body *body;

 

// Serial Port Stuff

SerialPort serialPort;

BYTE ReadBuffer[255];

DWORD BytesRead;

HANDLE hDataMutex;

bool dataChanged = true, dataChanged_c = true;

Vector3DF accOne;

Vector3DF accTwo;

UINT b[9] = {0,0,0,0,0,0,0,0,0};

int b_ptr = 0;

 

// File Logging

LogFile logFile;

bool logSerialToFile = false, readFromLogFile = false;

bool doneReadingLog = false;

OPENFILENAME ofn;

char szFileName[128];

BYTE readBuf[128];

 

void GameInit (void)

{

      // ------------- GAMEX BEGIN ---------------

      GameX.Initialize ("Motion Capture", RUN_NOCONFIRMQUIT | VIDEO_16BIT

            | VIDEO_WINDOWED | RUN_BACKGROUND | RUN_AUTOSHOWINFO, 1280, 768);

 

      view.x = 0; view.y = 0.5f; view.z = 250;

      axisView.x = 0; axisView.y = 0; axisView.z = 60;

 

      cam.SetWindow (0, 0, 512, 768);                 // Initialize Camera

      cam.SetToPosition( Vector3DF(0,0,0) );    // Set camera to look at the origin

      cam.SetFromAngles( view );

      GameX.SetDrawCam(&cam);                         // Tell GameX to draw using the camera

      // ------------- GAMEX END ---------------

 

 

      // ------------- GUI BEGIN ---------------

      startLogButton = new Button("Start Log", 10, 718, 100, 758);

      saveLogButton = new Button("Save Log", 110, 718, 200, 758);

      runLogButton = new Button("Run Log", 210, 718, 300, 758);

      resetButton = new Button("Reset", 310, 718, 400, 758);

 

      saveLogButton->Disable();

     

      hWnd = GameX.GetWindow();

      console = new Console("consoleLog.txt", 10,10,width-10,height-10);

      console->Printfn("Motion Capture Modelling System");

      console->Printfn("Debug Interface");

 

      accGraph[0] = new Graph("avg_acc1.x", -40, 40, width+0,   0, width+240, 190);

      accGraph[1] = new Graph("acc1.y", -40, 40, width+256, 0, width+496, 190);

      accGraph[2] = new Graph("acc1.z", -40, 40, width+512, 0, width+752, 190);

      velGraph[0] = new Graph("vel1.x", -4, 4, width+0,   192, width+240, 382);

      velGraph[1] = new Graph("vel1.y", -0.6, 0.6, width+256, 192, width+496, 382);

      velGraph[2] = new Graph("vel1.z", -0.6, 0.6, width+512, 192, width+752, 382);

      posGraph[0] = new Graph("pos1.x", -50, 50, width+0,   384, width+240, 574);

      posGraph[1] = new Graph("pos1.y", -50, 50, width+256, 384, width+496, 574);

      posGraph[2] = new Graph("pos1.z", -50, 50, width+512, 384, width+752, 574);

      angGraph[0] = new Graph("psi1",   -PI, PI,   width+0,   576, width+240, 766);

      angGraph[1] = new Graph("phi1",   -PI, PI, width+256, 576, width+496, 766);

      angGraph[2] = new Graph("theta1", -PI, PI, width+512, 576, width+752, 766);

      // ------------- GUI END ---------------

 

 

      // ------------- MOTION CAPTURE BEGIN ---------------

      shoulder = new Point( Vector3DF(0,0,0), 0, NULL, &cam);

      elbow = new Elbow( Vector3DF(0,0,-30), 30, shoulder, &cam);

      wrist = new Wrist( Vector3DF(0,-60,-30), 30, elbow, &cam); 

      body = new Body();

      // ------------- MOTION CAPTURE END ---------------

 

 

      // ------------- SERIAL PORT BEGIN ---------------

      if (serialPort.Init("COM1") == S_OK)

      {

            console->Printfn("COM1 Configured Correctly.");

      }

      else

      {

            // get error and display it

            char lpMsgBuf[100];

            serialPort.LastError(lpMsgBuf);

            console->Printfn("COM1 Configuration Error: %s", lpMsgBuf );

      }

      // ------------- SERIAL PORT END ---------------

 

      // ------------- LOG FILE START ---------------

      ofn.lStructSize = sizeof(OPENFILENAME);

      ofn.hwndOwner = hWnd;

      ofn.lpstrFilter = NULL;

      ofn.lpstrFile= szFileName;

      ofn.nMaxFile = sizeof(szFileName)/ sizeof(*szFileName);

      ofn.lpstrFileTitle = NULL;

      ofn.nMaxFileTitle = 0;

      ofn.lpstrInitialDir = (LPSTR)NULL;

      ofn.Flags = OFN_SHOWHELP | OFN_OVERWRITEPROMPT | OFN_CREATEPROMPT;

      ofn.lpstrTitle = NULL;

      // ------------- LOG FILE END ---------------

 

      wrist->Reset();

 

}

 

float wristPsi = 0;

float elbowPhi = 0;

 

void GameRun (void)

{

      DWORD dwWaitResult;

 

      if (readFromLogFile)

      {

            BytesRead = logFile.ReadLine(ReadBuffer);

            if (logFile.IsLogFinished())

            {

                  logFile.Close();

                  doneReadingLog = true;

                  readFromLogFile = false;

                  startLogButton->Enable();

                  runLogButton->Enable();

            }

      }

      else if( serialPort.Read(ReadBuffer, &BytesRead) == E_FAIL )

      {

            // get error and display it

            char lpMsgBuf[100];

            serialPort.LastError(lpMsgBuf);

 

            console->Printfn("Serial Data Read Error: %s", lpMsgBuf );

      }

 

      for(int i=0; i<BytesRead; i++)

      {

            // if buffer is 0x01

            if( ReadBuffer[i] == 0x01 )

                  b_ptr = 0;

            else

                  b[b_ptr++] = ReadBuffer[i];

 

            // if you have a full data packet

            if( b_ptr == 8 )

            {

                  b_ptr = 0;

 

                  b[7] = ReadBuffer[i];

 

                  // unpack the packet into values

                  accOne.x = (float)b[0] + (float)( (b[6]&0xC0)<<2 );

                  accOne.y = (float)b[1] + (float)( (b[6]&0x30)<<4 );

                  accOne.z = (float)b[2] + (float)( (b[6]&0x0C)<<6 );

 

                  accTwo.x = (float)b[3] + (float)( (b[6]&0x03)<<8 );

                  accTwo.y = (float)b[4] + (float)( (b[7]&0xC0)<<2 );

                  accTwo.z = (float)b[5] + (float)( (b[7]&0x30)<<4 );

 

                  // display the data

                  console->Printfn( "%f, %f, %f, %f, %f, %f", accOne.x, accOne.y, accOne.z, accTwo.x, accTwo.y, accTwo.z );

 

                  elbow->Run(accTwo);

                  wrist->Run(accOne);

 

                  body->RotateElbow( elbow->phi, elbow->theta );

                  body->RotateWrist( wrist->phi, wrist->psi);

                  //body->RotateElbow( elbowPhi, 0 );

                  //body->RotateWrist( wristPsi, PI/4);

                  //elbowPhi += (PI/1920);

                  //wristPsi += (PI/480);

 

                  if (!graphsDisabled)

                  {

                        accGraph[0]->AddPoint(wrist->x_avg);//acc.x);

                        accGraph[1]->AddPoint(wrist->acc.y);

                        accGraph[2]->AddPoint(wrist->acc.z);

 

                        velGraph[0]->AddPoint(wrist->vel.x);

                        velGraph[1]->AddPoint(wrist->vel.y);

                        velGraph[2]->AddPoint(wrist->vel.z);

 

                        posGraph[0]->AddPoint(wrist->pos.x);

                        posGraph[1]->AddPoint(wrist->pos.y);

                        posGraph[2]->AddPoint(wrist->pos.z);

 

                        angGraph[0]->AddPoint(wrist->psi);

                        angGraph[1]->AddPoint(wrist->phi);

                        angGraph[2]->AddPoint(wrist->theta);

                  }

            }

      }

 

      if (logSerialToFile)

      {

            for (int i = 0; i < BytesRead; i++)

                  logFile.Printf("%d ", ReadBuffer[i]);

            if (BytesRead > 0)

                  logFile.Printf("\n");

      }

 

      if (startLogButton->Run())

      {

            logFile.OpenForWriting("accLogFile_do_not_touch.txt");

            logSerialToFile = true;

            doneReadingLog = false;

            saveLogButton->Enable();

      }

      if (saveLogButton->Run())

      {

            BOOL saveOK = GetSaveFileName(&ofn);

            if (saveOK)

            {

                  logFile.Close();

                  logSerialToFile = false;

                  doneReadingLog = false;

                  saveLogButton->Disable();

                  CopyFile("accLogFile_do_not_touch.txt", ofn.lpstrFile, FALSE);

            }    

      }

      if (runLogButton->Run() && !logSerialToFile)

      {

            BOOL openOK = GetOpenFileName(&ofn);

            if (openOK)

            {

                  logFile.OpenForReading(ofn.lpstrFile);

                  readFromLogFile = true;

                  doneReadingLog = false;

                  startLogButton->Disable();

                  saveLogButton->Disable();

                  runLogButton->Disable();

            }    

      }

      if (resetButton->Run())

      {

            wrist->Reset();

            elbow->Reset();

            body->Reset();

      }

 

      // move camera view about the origin on right button down

      if( GameX.IsMouseDown(MOUSE_RIGHT) ){

 

            view.x += GameX.GetMouseDX();

            view.y += GameX.GetMouseDY();

            view.z += GameX.GetMouseDZ()/20;

 

            if( view.y < -89.9f ) view.y = -89.9f;

            if( view.y > 89.9f )  view.y = 89.9f;

            if( view.z < 2.0f )     view.z = 2.0f;

 

            cam.SetToPosition( Vector3DF(0,0,0) );

            cam.SetFromAngles( view );

            axisView.x = view.x;

            axisView.y = view.y;

      }

 

      // Toggle Console on '`' key press

      if ( GameX.GetKeyPress(KEY_TILDE) )

            console->Toggle();

 

      if ( GameX.GetKeyPress(KEY_0) )

            graphsDisabled = !graphsDisabled;

 

      if (!graphsDisabled)

      {

            if ( GameX.GetKeyPress(KEY_1) )

                  accGraph[0]->Toggle();

            if ( GameX.GetKeyPress(KEY_2) )

                  accGraph[1]->Toggle();

            if ( GameX.GetKeyPress(KEY_3) )

                  accGraph[2]->Toggle();

            if ( GameX.GetKeyPress(KEY_Q) )

                  velGraph[0]->Toggle();

            if ( GameX.GetKeyPress(KEY_W) )

                  velGraph[1]->Toggle();

            if ( GameX.GetKeyPress(KEY_E) )

                  velGraph[2]->Toggle();

            if ( GameX.GetKeyPress(KEY_A) )

                  posGraph[0]->Toggle();

            if ( GameX.GetKeyPress(KEY_S) )

                  posGraph[1]->Toggle();

            if ( GameX.GetKeyPress(KEY_D) )

                  posGraph[2]->Toggle();

            if ( GameX.GetKeyPress(KEY_Z) )

                  angGraph[0]->Toggle();

            if ( GameX.GetKeyPress(KEY_X) )

                  angGraph[1]->Toggle();

            if ( GameX.GetKeyPress(KEY_C) )

                  angGraph[2]->Toggle();

 

            if ( GameX.GetKeyPress(KEY_9) )

            {

                  showAllGraphs = !showAllGraphs;

                  accGraph[0]->SetShowGraph(showAllGraphs);

                  accGraph[1]->SetShowGraph(showAllGraphs);

                  accGraph[2]->SetShowGraph(showAllGraphs);

                  velGraph[0]->SetShowGraph(showAllGraphs);

                  velGraph[1]->SetShowGraph(showAllGraphs);

                  velGraph[2]->SetShowGraph(showAllGraphs);

                  posGraph[0]->SetShowGraph(showAllGraphs);

                  posGraph[1]->SetShowGraph(showAllGraphs);

                  posGraph[2]->SetShowGraph(showAllGraphs);

                  angGraph[0]->SetShowGraph(showAllGraphs);

                  angGraph[1]->SetShowGraph(showAllGraphs);

                  angGraph[2]->SetShowGraph(showAllGraphs);

            }

      }

}

 

void drawAxis ()

{

      GameX.DrawLine(ColorX(150,150,150), 0, height, width, height);

      GameX.DrawLine(ColorX(150,150,150), width, 0, width, height);

 

      cam.SetWindow ( width-100, 0, width, 100 );

      cam.SetFromAngles( axisView );

     

      GameX.DrawLine3D (ColorX(255,0,0), Vector3DF(0,0,0), Vector3DF(24,0,0));

      GameX.DrawLine3D (ColorX(0,255,0), Vector3DF(0,0,0), Vector3DF(0,24,0));

      GameX.DrawLine3D (ColorX(0,0,255), Vector3DF(0,0,0), Vector3DF(0,0,24));

 

      float x, y, z;

      cam.Project( Vector3DF(24,0,0), x, y, z);

      GameX.DrawText(x-5, y-5, txt_x);

      cam.Project( Vector3DF(0,24,0), x, y, z);

      GameX.DrawText(x-5, y-5, txt_y);

      cam.Project( Vector3DF(0,0,24), x, y, z);

      GameX.DrawText(x-5, y-5, txt_z);

 

      cam.SetWindow (0, 0, width, height);

      cam.SetFromAngles( view );

 

      sprintf(buffer, "x:%5.3f, y:%5.3f, z:%5.3f", wrist->pos.x, wrist->pos.y, wrist->pos.z);

      GameX.DrawText(5,5,buffer);  

      sprintf(buffer, "vx:%5.3f, vy:%5.3f, vz:%5.3f", wrist->vel.x, wrist->vel.y, wrist->vel.z);

      GameX.DrawText(5,20,buffer); 

      sprintf(buffer, "ax:%5.3f, ay:%5.3f, az:%5.3f", wrist->x_avg, wrist->acc.y, wrist->acc.z);

      GameX.DrawText(5,35,buffer);

      sprintf(buffer, "phi:%5.3f, psi:%5.3f , theta:%5.3f", wrist->phi, wrist->psi, wrist->theta);

      GameX.DrawText(5,50,buffer);

}

 

void GameDraw (void)

{

      GameX.ClearScreen();

     

      // Draw Gray Background

      GameX.SetView(0,0,width,height);

      GameX.FillScreen (ColorX(100,100,100));

      GameX.ResetView();

 

      GameX.Begin3DScene ();

 

      body->Draw();

 

      //wrist->Draw();

      //elbow->Draw();

      shoulder->Draw();

 

      if (!graphsDisabled)

      {

            accGraph[0]->Draw();

            accGraph[1]->Draw();

            accGraph[2]->Draw();

            velGraph[0]->Draw();

            velGraph[1]->Draw();

            velGraph[2]->Draw();

            posGraph[0]->Draw();

            posGraph[1]->Draw();

            posGraph[2]->Draw();

            angGraph[0]->Draw();

            angGraph[1]->Draw();

            angGraph[2]->Draw();

      }

 

      startLogButton->Draw();

      saveLogButton->Draw();

      runLogButton->Draw();

      resetButton->Draw();

 

      if (logSerialToFile)

            GameX.DrawText(10, 700, "Logging...");

 

      if (readFromLogFile)

            GameX.DrawText(10, 700, "Reading from log file...");

 

      if (doneReadingLog)

            GameX.DrawText(10, 700, "Done Reading from log file!");

     

      drawAxis ();

 

      GameX.End3DScene ();

 

      console->Draw(); 

}

 

A.2.2 Body.hpp

 

#include "GameX.hpp"

 

#define PI  3.14159f

#define PI_2      1.570795f

 

class Body

{

private:

      ImageX boxImg;

      ImageX redImg;

      ImageX blueImg;

      ImageX bigRedImg;

      ImageX faceImg1;

      ImageX faceImg2;

      ImageX faceImg3;

 

      float elbow_phi, elbow_theta, wrist_phi, wrist_psi;

     

      Vector3DF fe1_pt[8];

      Vector3DF fe2_pt[8];

      Vector3DF sh1_pt[8];

      Vector3DF sh2_pt[8];

      Vector3DF th1_pt[8];

      Vector3DF th2_pt[8];

      Vector3DF pel_pt[8];

      Vector3DF tor_pt[8];

      Vector3DF sho_pt[8];

      Vector3DF nec_pt[8];

      Vector3DF hed_pt[8];

 

      Vector3DF upArm1_pt[8];

      Vector3DF upArm2_pt[8];

      Vector3DF loArm1_pt[8];

      Vector3DF loArm2_pt[8];

      Vector3DF hand1_pt[8];

      Vector3DF hand2_pt[8];

 

      Vector3DF orig_fe1_pt[8];

      Vector3DF orig_fe2_pt[8];

      Vector3DF orig_sh1_pt[8];

      Vector3DF orig_sh2_pt[8];

      Vector3DF orig_th1_pt[8];

      Vector3DF orig_th2_pt[8];

      Vector3DF orig_pel_pt[8];

      Vector3DF orig_tor_pt[8];

      Vector3DF orig_sho_pt[8];

      Vector3DF orig_nec_pt[8];

      Vector3DF orig_hed_pt[8];

 

      Vector3DF orig_upArm1_pt[8];

      Vector3DF orig_upArm2_pt[8];

      Vector3DF orig_loArm1_pt[8];

      Vector3DF orig_loArm2_pt[8];

      Vector3DF orig_hand1_pt[8];

      Vector3DF orig_hand2_pt[8];

 

      Vector3DF orig_loArm1_avg, orig_hand1_avg;

      Vector3DF tempPt;

 

      void DrawBoxPts(Vector3DF, Vector3DF, Vector3DF, Vector3DF, Vector3DF, Vector3DF, Vector3DF, Vector3DF);

      void DrawBoxPts(ImageX *img, Vector3DF pt[]);

      void DrawFace( Vector3DF pt[] );

      void DrawShirt( Vector3DF pt[] );

 

      void RotateAroundPoint(Vector3DF& position, Vector3DF vCenter, float angle, Vector3DF vAxis);

      void RotateAroundAxisX(Vector3DF &newPosition, Vector3DF vPoint, Vector3DF vCenter, float angle);

      void RotateAroundAxisY(Vector3DF &newPosition, Vector3DF vPoint, Vector3DF vCenter, float angle);

 

public:

      Body();

      void Draw();

      void RotateElbow(float phi, float theta);

      void RotateWrist(float phi, float psi);

      void Reset();

};

 

 

 

A.2.3 Body.cpp

 

#include "Body.hpp"

 

#define THICK           4           // Thickness of Body (y-axis)

 

#define UPARM_W1_1      18          // Top inner x-cord

#define UPARM_W1_2      25          // Top outer x-cord

#define UPARM_W2_1      18          // Bottom inner x-cord

#define UPARM_W2_2      25          // Bottom outer x-cord

#define UPARM_H1  50          // Top Height

#define UPARM_H2  15          // Bottom Height

#define UPARM_DIST      (UPARM_H1-UPARM_H2)

 

#define LOARM_W1_1      18          // Top inner x-cord

#define LOARM_W1_2      25          // Top outer x-cord

#define LOARM_W2_1      18          // Bottom inner x-cord

#define LOARM_W2_2      25          // Bottom outer x-cord

#define LOARM_H1  15          // Top Height

#define LOARM_H2  -10         // Bottom Height

#define LOARM_DIST      (LOARM_H1-LOARM_H2)

 

#define HAND_W1_1 18          // Top inner x-cord

#define HAND_W1_2 25          // Top outer x-cord

#define HAND_W2_1 18          // Bottom inner x-cord

#define HAND_W2_2 24          // Bottom outer x-cord

#define HAND_H1         -10         // Top Height

#define HAND_H2         -25         // Bottom Height

#define HAND_DIST (HAND_H1-HAND_H2)

#define HAND_THICK      7

 

#define SHO_W1          18          // Top x-cord

#define SHO_W2          18          // Bottom x-cord

#define SHO_H1          50          // Top Height

#define SHO_H2          40          // Bottom Height

 

#define TOR_W1          15          // Top x-cord

#define TOR_W2          12          // Bottom x-cord

#define TOR_H1          40          // Top Height

#define TOR_H2          0           // Bottom Height

 

#define PEL_W1          12          // Top x-cord

#define PEL_W2          12          // Bottom x-cord

#define PEL_H1          0           // Top Height

#define PEL_H2          -10         // Bottom Height

 

#define THI_W1_1  2           // Top inner x-cord

#define THI_W1_2  12          // Top outer x-cord

#define THI_W2_1  2           // Bottom inner x-cord

#define THI_W2_2  12          // Bottom outer x-cord

#define THI_H1          -10         // Top Height

#define THI_H2          -50         // Bottom Height

 

#define SHIN_W1_1 2           // Top inner x-cord

#define SHIN_W1_2 12          // Top outer x-cord

#define SHIN_W2_1 2           // Bottom inner x-cord

#define SHIN_W2_2 12          // Bottom outer x-cord

#define SHIN_H1         -50         // Top Height

#define SHIN_H2         -90         // Bottom Height

 

#define FEET_W1_1 2           // Top inner x-cord

#define FEET_W1_2 12          // Top outer x-cord

#define FEET_W2_1 2           // Bottom inner x-cord

#define FEET_W2_2 12          // Bottom outer x-cord

#define FEET_H1         -90         // Top Height

#define FEET_H2         -95         // Bottom Height

#define FEET_THICK      10

 

Body::Body()

{

      boxImg.Create(128,128,false);

      boxImg.Fill(150,150,150);

      for (int i=0; i < 128; i++)

      {

            boxImg.SetPixel(i, 0, 0, 0, 0);

            boxImg.SetPixel(i, 127, 0, 0, 0);

            boxImg.SetPixel(0, i, 0, 0, 0);

            boxImg.SetPixel(127, i, 0, 0, 0);

      }

 

      redImg.Create(128,128,false);

      redImg.Fill(255,0,0);

      for (int i=0; i < 128; i++)

      {

            redImg.SetPixel(i, 0, 0, 0, 0);

            redImg.SetPixel(i, 127, 0, 0, 0);

            redImg.SetPixel(0, i, 0, 0, 0);

            redImg.SetPixel(127, i, 0, 0, 0);

      }

 

      blueImg.Create(128,128,false);

      blueImg.Fill(0,0,255);

      for (int i=0; i < 128; i++)

      {

            blueImg.SetPixel(i, 0, 0, 0, 0);

            blueImg.SetPixel(i, 127, 0, 0, 0);

            blueImg.SetPixel(0, i, 0, 0, 0);

            blueImg.SetPixel(127, i, 0, 0, 0);

      }

 

      faceImg1.Create(128,128,false);

      faceImg1.Fill(250,250,0);

      for (int i=0; i < 128; i++)

      {

            faceImg1.SetPixel(i, 0, 0, 0, 0);

            faceImg1.SetPixel(i, 127, 0, 0, 0);

            faceImg1.SetPixel(0, i, 0, 0, 0);

            faceImg1.SetPixel(127, i, 0, 0, 0);

      }

 

      faceImg2.Load("Images\\Smiley.bmp",false);

      faceImg3.Load("Images\\NotSmiley.bmp",false);

 

      bigRedImg.Load("Images\\BigRed.bmp",false);

 

      elbow_phi = 0;

      elbow_theta = 0;

      wrist_phi = 0;

      wrist_psi = 0;

 

      // Feet

      fe1_pt[0].Set(-FEET_W1_2,-THICK-FEET_THICK, FEET_H1);

      fe1_pt[1].Set(-FEET_W1_1,-THICK-FEET_THICK, FEET_H1);

      fe1_pt[2].Set(-FEET_W1_2, THICK,                FEET_H1);

      fe1_pt[3].Set(-FEET_W1_1, THICK,                FEET_H1);

      fe1_pt[4].Set(-FEET_W2_2,-THICK-FEET_THICK, FEET_H2);

      fe1_pt[5].Set(-FEET_W2_1,-THICK-FEET_THICK, FEET_H2);

      fe1_pt[6].Set(-FEET_W2_2, THICK,                FEET_H2);

      fe1_pt[7].Set(-FEET_W2_1, THICK,                FEET_H2);

 

      fe2_pt[0].Set( FEET_W1_2,-THICK-FEET_THICK, FEET_H1);

      fe2_pt[1].Set( FEET_W1_1,-THICK-FEET_THICK, FEET_H1);

      fe2_pt[2].Set( FEET_W1_2, THICK,                FEET_H1);

      fe2_pt[3].Set( FEET_W1_1, THICK,                FEET_H1);

      fe2_pt[4].Set( FEET_W2_2,-THICK-FEET_THICK, FEET_H2);

      fe2_pt[5].Set( FEET_W2_1,-THICK-FEET_THICK, FEET_H2);

      fe2_pt[6].Set( FEET_W2_2, THICK,                FEET_H2);

      fe2_pt[7].Set( FEET_W2_1, THICK,                FEET_H2);

 

      // Shin

      sh1_pt[0].Set(-SHIN_W1_2,-THICK, SHIN_H1);

      sh1_pt[1].Set(-SHIN_W1_1,-THICK, SHIN_H1);

      sh1_pt[2].Set(-SHIN_W1_2, THICK, SHIN_H1);

      sh1_pt[3].Set(-SHIN_W1_1, THICK, SHIN_H1);

      sh1_pt[4].Set(-SHIN_W2_2,-THICK, SHIN_H2);

      sh1_pt[5].Set(-SHIN_W2_1,-THICK, SHIN_H2);

      sh1_pt[6].Set(-SHIN_W2_2, THICK, SHIN_H2);

      sh1_pt[7].Set(-SHIN_W2_1, THICK, SHIN_H2);

 

      sh2_pt[0].Set( SHIN_W1_1,-THICK, SHIN_H1);

      sh2_pt[1].Set( SHIN_W1_2,-THICK, SHIN_H1);

      sh2_pt[2].Set( SHIN_W1_1, THICK, SHIN_H1);

      sh2_pt[3].Set( SHIN_W1_2, THICK, SHIN_H1);

      sh2_pt[4].Set( SHIN_W2_1,-THICK, SHIN_H2);

      sh2_pt[5].Set( SHIN_W2_2,-THICK, SHIN_H2);

      sh2_pt[6].Set( SHIN_W2_1, THICK, SHIN_H2);

      sh2_pt[7].Set( SHIN_W2_2, THICK, SHIN_H2);

 

      // Thigh

      th1_pt[0].Set(-THI_W1_2,-THICK, THI_H1);

      th1_pt[1].Set(-THI_W1_1,-THICK, THI_H1);

      th1_pt[2].Set(-THI_W1_2, THICK, THI_H1);

      th1_pt[3].Set(-THI_W1_1, THICK, THI_H1);

      th1_pt[4].Set(-THI_W2_2,-THICK, THI_H2);

      th1_pt[5].Set(-THI_W2_1,-THICK, THI_H2);

      th1_pt[6].Set(-THI_W2_2, THICK, THI_H2);

      th1_pt[7].Set(-THI_W2_1, THICK, THI_H2);

 

      th2_pt[0].Set( THI_W1_1,-THICK, THI_H1);

      th2_pt[1].Set( THI_W1_2,-THICK, THI_H1);

      th2_pt[2].Set( THI_W1_1, THICK, THI_H1);

      th2_pt[3].Set( THI_W1_2, THICK, THI_H1);

      th2_pt[4].Set( THI_W2_1,-THICK, THI_H2);

      th2_pt[5].Set( THI_W2_2,-THICK, THI_H2);

      th2_pt[6].Set( THI_W2_1, THICK, THI_H2);

      th2_pt[7].Set( THI_W2_2, THICK, THI_H2);

 

      // Pelvis

      pel_pt[0].Set(-PEL_W1,-THICK, PEL_H1);

      pel_pt[1].Set( PEL_W1,-THICK, PEL_H1);

      pel_pt[2].Set(-PEL_W1, THICK, PEL_H1);

      pel_pt[3].Set( PEL_W1, THICK, PEL_H1);

      pel_pt[4].Set(-PEL_W2,-THICK, PEL_H2);

      pel_pt[5].Set( PEL_W2,-THICK, PEL_H2);

      pel_pt[6].Set(-PEL_W2, THICK, PEL_H2);

      pel_pt[7].Set( PEL_W2, THICK, PEL_H2);

 

      // Torso

      tor_pt[0].Set(-TOR_W1,-THICK, TOR_H1);

      tor_pt[1].Set( TOR_W1,-THICK, TOR_H1);

      tor_pt[2].Set(-TOR_W1, THICK, TOR_H1);

      tor_pt[3].Set( TOR_W1, THICK, TOR_H1);

      tor_pt[4].Set(-TOR_W2,-THICK, TOR_H2);

      tor_pt[5].Set( TOR_W2,-THICK, TOR_H2);

      tor_pt[6].Set(-TOR_W2, THICK, TOR_H2);

      tor_pt[7].Set( TOR_W2, THICK, TOR_H2);

 

      // Shoulder

      sho_pt[0].Set(-SHO_W1,-THICK, SHO_H1);

      sho_pt[1].Set( SHO_W1,-THICK, SHO_H1);

      sho_pt[2].Set(-SHO_W1, THICK, SHO_H1);

      sho_pt[3].Set( SHO_W1, THICK, SHO_H1);

      sho_pt[4].Set(-SHO_W2,-THICK, SHO_H2);

      sho_pt[5].Set( SHO_W2,-THICK, SHO_H2);

      sho_pt[6].Set(-SHO_W2, THICK, SHO_H2);

      sho_pt[7].Set( SHO_W2, THICK, SHO_H2);

 

      // Neck

      nec_pt[0].Set(-2,-2, 55);

      nec_pt[1].Set( 2,-2, 55);

      nec_pt[2].Set(-2, 2, 55);

      nec_pt[3].Set( 2, 2, 55);

      nec_pt[4].Set(-2,-2, 50);

      nec_pt[5].Set( 2,-2, 50);

      nec_pt[6].Set(-2, 2, 50);

      nec_pt[7].Set( 2, 2, 50);

 

      // Head

      hed_pt[0].Set(-10,-THICK, 80);

      hed_pt[1].Set( 10,-THICK, 80);

      hed_pt[2].Set(-10, THICK, 80);

      hed_pt[3].Set( 10, THICK, 80);

      hed_pt[4].Set(-7,-THICK, 55);

      hed_pt[5].Set( 7,-THICK, 55);

      hed_pt[6].Set(-7, THICK, 55);

      hed_pt[7].Set( 7, THICK, 55);

 

      // Arms

      upArm1_pt[0].Set(-UPARM_W1_2,-THICK, UPARM_H1);

      upArm1_pt[1].Set(-UPARM_W1_1,-THICK, UPARM_H1);

      upArm1_pt[2].Set(-UPARM_W1_2, THICK, UPARM_H1);

      upArm1_pt[3].Set(-UPARM_W1_1, THICK, UPARM_H1);

      upArm1_pt[4].Set(-UPARM_W2_2,-THICK, UPARM_H2);

      upArm1_pt[5].Set(-UPARM_W2_1,-THICK, UPARM_H2);

      upArm1_pt[6].Set(-UPARM_W2_2, THICK, UPARM_H2);

      upArm1_pt[7].Set(-UPARM_W2_1, THICK, UPARM_H2);

 

      upArm2_pt[0].Set( UPARM_W1_2,-THICK, UPARM_H1);

      upArm2_pt[1].Set( UPARM_W1_1,-THICK, UPARM_H1);

      upArm2_pt[2].Set( UPARM_W1_2, THICK, UPARM_H1);

      upArm2_pt[3].Set( UPARM_W1_1, THICK, UPARM_H1);

      upArm2_pt[4].Set( UPARM_W2_2,-THICK, UPARM_H2);

      upArm2_pt[5].Set( UPARM_W2_1,-THICK, UPARM_H2);

      upArm2_pt[6].Set( UPARM_W2_2, THICK, UPARM_H2);

      upArm2_pt[7].Set( UPARM_W2_1, THICK, UPARM_H2);

 

      loArm1_pt[0].Set(-LOARM_W1_2,-THICK, LOARM_H1);

      loArm1_pt[1].Set(-LOARM_W1_1,-THICK, LOARM_H1);

      loArm1_pt[2].Set(-LOARM_W1_2, THICK, LOARM_H1);

      loArm1_pt[3].Set(-LOARM_W1_1, THICK, LOARM_H1);

      loArm1_pt[4].Set(-LOARM_W2_2,-THICK, LOARM_H2);

      loArm1_pt[5].Set(-LOARM_W2_1,-THICK, LOARM_H2);

      loArm1_pt[6].Set(-LOARM_W2_2, THICK, LOARM_H2);

      loArm1_pt[7].Set(-LOARM_W2_1, THICK, LOARM_H2);

 

      loArm2_pt[0].Set( LOARM_W1_2,-THICK, LOARM_H1);

      loArm2_pt[1].Set( LOARM_W1_1,-THICK, LOARM_H1);

      loArm2_pt[2].Set( LOARM_W1_2, THICK, LOARM_H1);

      loArm2_pt[3].Set( LOARM_W1_1, THICK, LOARM_H1);

      loArm2_pt[4].Set( LOARM_W2_2,-THICK, LOARM_H2);

      loArm2_pt[5].Set( LOARM_W2_1,-THICK, LOARM_H2);

      loArm2_pt[6].Set( LOARM_W2_2, THICK, LOARM_H2);

      loArm2_pt[7].Set( LOARM_W2_1, THICK, LOARM_H2);

 

      hand1_pt[0].Set(-HAND_W1_2,-HAND_THICK, HAND_H1);

      hand1_pt[1].Set(-HAND_W1_1,-HAND_THICK, HAND_H1);

      hand1_pt[2].Set(-HAND_W1_2, HAND_THICK, HAND_H1);

      hand1_pt[3].Set(-HAND_W1_1, HAND_THICK, HAND_H1);

      hand1_pt[4].Set(-HAND_W2_2,-HAND_THICK, HAND_H2);

      hand1_pt[5].Set(-HAND_W2_1,-HAND_THICK, HAND_H2);

      hand1_pt[6].Set(-HAND_W2_2, HAND_THICK, HAND_H2);

      hand1_pt[7].Set(-HAND_W2_1, HAND_THICK, HAND_H2);

 

      hand2_pt[0].Set( HAND_W1_2,-HAND_THICK, HAND_H1);

      hand2_pt[1].Set( HAND_W1_1,-HAND_THICK, HAND_H1);

      hand2_pt[2].Set( HAND_W1_2, HAND_THICK, HAND_H1);

      hand2_pt[3].Set( HAND_W1_1, HAND_THICK, HAND_H1);

      hand2_pt[4].Set( HAND_W2_2,-HAND_THICK, HAND_H2);

      hand2_pt[5].Set( HAND_W2_1,-HAND_THICK, HAND_H2);

      hand2_pt[6].Set( HAND_W2_2, HAND_THICK, HAND_H2);

      hand2_pt[7].Set( HAND_W2_1, HAND_THICK, HAND_H2);

 

      for (int i = 0; i < 8; i++)

      {

            orig_fe1_pt[i] = fe1_pt[i];

            orig_fe2_pt[i] = fe2_pt[i];

            orig_sh1_pt[i] = sh1_pt[i];

            orig_sh2_pt[i] = sh2_pt[i];

            orig_th1_pt[i] = th1_pt[i];

            orig_th2_pt[i] = th2_pt[i];

            orig_pel_pt[i] = pel_pt[i];

            orig_tor_pt[i] = tor_pt[i];

            orig_sho_pt[i] = sho_pt[i];

            orig_nec_pt[i] = nec_pt[i];

            orig_hed_pt[i] = hed_pt[i];

            orig_upArm1_pt[i] = upArm1_pt[i];

            orig_upArm2_pt[i] = upArm2_pt[i];

            orig_loArm1_pt[i] = loArm1_pt[i];

            orig_loArm2_pt[i] = loArm2_pt[i];

            orig_hand1_pt[i] = hand1_pt[i];

            orig_hand2_pt[i] = hand2_pt[i];

      }

 

      float avgX, avgY, avgZ;

      avgX = (orig_loArm1_pt[0].x + orig_loArm1_pt[1].x + orig_loArm1_pt[2].x + orig_loArm1_pt[3].x) / 4;

      avgY = (orig_loArm1_pt[0].y + orig_loArm1_pt[1].y + orig_loArm1_pt[2].y + orig_loArm1_pt[3].y) / 4;

      avgZ = (orig_loArm1_pt[0].z + orig_loArm1_pt[1].z + orig_loArm1_pt[2].z + orig_loArm1_pt[3].z) / 4;

      orig_loArm1_avg.Set(avgX, avgY, avgZ);

 

      avgX = (orig_hand1_pt[0].x + orig_hand1_pt[1].x + orig_hand1_pt[2].x + orig_hand1_pt[3].x) / 4;

      avgY = (orig_hand1_pt[0].y + orig_hand1_pt[1].y + orig_hand1_pt[2].y + orig_hand1_pt[3].y) / 4;

      avgZ = (orig_hand1_pt[0].z + orig_hand1_pt[1].z + orig_hand1_pt[2].z + orig_hand1_pt[3].z) / 4;

      orig_hand1_avg.Set(avgX, avgY, avgZ);

 

      Reset();

 

}

 

void Body::Draw()

{

      // Feet

      DrawBoxPts(&boxImg, fe1_pt);

      DrawBoxPts(&boxImg, fe2_pt);

     

      // Shins

      DrawBoxPts(&boxImg, sh1_pt);

      DrawBoxPts(&boxImg, sh2_pt);

 

      // Thighs

      DrawBoxPts(&blueImg, th1_pt);

      DrawBoxPts(&blueImg, th2_pt);

 

      // Pelvis

      DrawBoxPts(&blueImg, pel_pt);

 

      // Torso

      //DrawBoxPts(&boxImg, tor_pt);

      DrawShirt(tor_pt);

 

      // Shoulder

      DrawBoxPts(&redImg, sho_pt);

 

      // Neck

      DrawBoxPts(&boxImg, nec_pt);

 

      // Arms

      DrawBoxPts(&redImg, upArm1_pt);

      DrawBoxPts(&redImg, upArm2_pt);

      DrawBoxPts(&redImg, loArm1_pt);

      DrawBoxPts(&redImg, loArm2_pt);

 

      // Hands

      DrawBoxPts(&boxImg, hand1_pt);

      DrawBoxPts(&boxImg, hand2_pt);

 

      // Head

      DrawFace(hed_pt);

 

      //GameX.DrawPoint3D(ColorX(255,255,255), tempPt);

      //GameX.DrawLine3D(ColorX(255,255,255), Vector3DF(50,0,50), Vector3DF(50,0,50)+ tempPt);

}

 

void Body::DrawBoxPts(Vector3DF pt1, Vector3DF pt2, Vector3DF pt3, Vector3DF pt4, Vector3DF pt5, Vector3DF pt6, Vector3DF pt7, Vector3DF pt8)

{

      GameX.DrawTexturedPolygon3D( &boxImg, pt3, pt4, pt1, pt2 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt5, pt6, pt7, pt8 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt1, pt2, pt5, pt6 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt2, pt4, pt6, pt8 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt4, pt3, pt8, pt7 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt3, pt1, pt7, pt5 );

}

 

void Body::DrawBoxPts(ImageX *img, Vector3DF pt[])

{

      GameX.DrawTexturedPolygon3D( img, pt[2], pt[3], pt[0], pt[1] );

      GameX.DrawTexturedPolygon3D( img, pt[4], pt[5], pt[6], pt[7] );

      GameX.DrawTexturedPolygon3D( img, pt[0], pt[1], pt[4], pt[5] );

      GameX.DrawTexturedPolygon3D( img, pt[1], pt[3], pt[5], pt[7] );

      GameX.DrawTexturedPolygon3D( img, pt[3], pt[2], pt[7], pt[6] );

      GameX.DrawTexturedPolygon3D( img, pt[2], pt[0], pt[6], pt[4] );

}

 

void Body::DrawFace( Vector3DF pt[] )

{

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[2], pt[3], pt[0], pt[1] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[4], pt[5], pt[6], pt[7] );

      if (wrist_phi > elbow_phi)

            GameX.DrawTexturedPolygon3D( &faceImg3, pt[0], pt[1], pt[4], pt[5] );

      else

            GameX.DrawTexturedPolygon3D( &faceImg2, pt[0], pt[1], pt[4], pt[5] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[1], pt[3], pt[5], pt[7] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[3], pt[2], pt[7], pt[6] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[2], pt[0], pt[6], pt[4] );

}

 

void Body::DrawShirt( Vector3DF pt[] )

{

      GameX.DrawTexturedPolygon3D( &redImg, pt[2], pt[3], pt[0], pt[1] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[4], pt[5], pt[6], pt[7] );

      GameX.DrawTexturedPolygon3D( &bigRedImg, pt[0], pt[1], pt[4], pt[5] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[1], pt[3], pt[5], pt[7] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[3], pt[2], pt[7], pt[6] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[2], pt[0], pt[6], pt[4] );

}

 

void Body::RotateAroundPoint(Vector3DF& position, Vector3DF vCenter, float angle, Vector3DF vAxis)

{

      Vector3DF vNewPosition;

 

      // Get the Vector from our position to the center we are rotating around

      Vector3DF vPos = position - vCenter;

 

      // Calculate the sine and cosine of the angle once

      float cosTheta = (float)cos(angle);

      float sinTheta = (float)sin(angle);

 

      // Find the new x position for the new rotated point

      vNewPosition.x  = (cosTheta + (1 - cosTheta) * vAxis.x * vAxis.x)       * vPos.x;

      vNewPosition.x += ((1 - cosTheta) * vAxis.x * vAxis.y - vAxis.z * sinTheta)   * vPos.y;

      vNewPosition.x += ((1 - cosTheta) * vAxis.x * vAxis.z + vAxis.y * sinTheta)   * vPos.z;

 

      // Find the new y position for the new rotated point

      vNewPosition.y  = ((1 - cosTheta) * vAxis.x * vAxis.y + vAxis.z * sinTheta)   * vPos.x;

      vNewPosition.y += (cosTheta + (1 - cosTheta) * vAxis.y * vAxis.y)       * vPos.y;

      vNewPosition.y += ((1 - cosTheta) * vAxis.y * vAxis.z - vAxis.x * sinTheta)   * vPos.z;

 

      // Find the new z position for the new rotated point

      vNewPosition.z  = ((1 - cosTheta) * vAxis.x * vAxis.z - vAxis.y * sinTheta)   * vPos.x;

      vNewPosition.z += ((1 - cosTheta) * vAxis.y * vAxis.z + vAxis.x * sinTheta)   * vPos.y;

      vNewPosition.z += (cosTheta + (1 - cosTheta) * vAxis.z * vAxis.z)       * vPos.z;

 

      position = vCenter + vNewPosition;

}

 

void Body::RotateAroundAxisX(Vector3DF &newPosition, Vector3DF vPoint, Vector3DF vCenter, float angle)

{

      Vector3DF vNewPosition;

 

      // Get the Vector from our position to the center we are rotating around

      Vector3DF vPos = vPoint - vCenter;

 

      // Calculate the sine and cosine of the angle once

      float cosTheta = (float)cos(angle);

      float sinTheta = (float)sin(angle);

 

      // Find the new x position for the new rotated point

      vNewPosition.x  = vPos.x;

 

      // Find the new y position for the new rotated point

      vNewPosition.y  = (cosTheta * vPos.y) - (sinTheta * vPos.z);

 

      // Find the new z position for the new rotated point

      vNewPosition.z  = (sinTheta * vPos.y) + (cosTheta * vPos.z);

 

      newPosition = (vCenter + vNewPosition);

}

 

void Body::RotateAroundAxisY(Vector3DF &newPosition, Vector3DF vPoint, Vector3DF vCenter, float angle)

{

      Vector3DF vNewPosition;

 

      // Get the Vector from our position to the center we are rotating around

      Vector3DF vPos = vPoint - vCenter;

 

      // Calculate the sine and cosine of the angle once

      float cosTheta = (float)cos(angle);

      float sinTheta = (float)sin(angle);

 

      // Find the new x position for the new rotated point

      vNewPosition.x  = (cosTheta * vPos.x) + (sinTheta * vPos.z);

 

      // Find the new y position for the new rotated point

      vNewPosition.y  = vPos.y;

 

      // Find the new z position for the new rotated point

      vNewPosition.z  = (cosTheta * vPos.z) - (sinTheta * vPos.x);

 

      newPosition = vCenter + vNewPosition;

}

 

 

 

void Body::RotateElbow(float phi, float theta)

{

      theta *= 1/60.0f;

      phi *= -1.0f/60.0f;

      phi -= PI_2;

 

      Vector3DF rotatePoint( (-UPARM_W1_1-UPARM_W1_2)/2, 0, UPARM_H1-THICK );

 

      for (int i = 0; i < 8; i++)

      {

            RotateAroundAxisY( upArm1_pt[i],    orig_upArm1_pt[i],            rotatePoint, theta);

            RotateAroundAxisY( loArm1_pt[i],    orig_loArm1_pt[i],            rotatePoint, theta);

            RotateAroundAxisY( hand1_pt[i],           orig_hand1_pt[i],       rotatePoint, theta);

      }

     

      for (int i = 0; i < 8; i++)

      {

            RotateAroundAxisX( upArm1_pt[i],    upArm1_pt[i],     rotatePoint, phi );

            RotateAroundAxisX( loArm1_pt[i],    loArm1_pt[i],     rotatePoint, phi );

            RotateAroundAxisX( hand1_pt[i],           hand1_pt[i],      rotatePoint, phi );

      }

 

      elbow_phi = phi;

      elbow_theta = theta;

}

 

void Body::RotateWrist(float phi, float psi)

{

      phi *= -1.0f;

      phi -= PI_2;

 

      float avgX, avgY, avgZ;

      avgX = (loArm1_pt[0].x + loArm1_pt[1].x + loArm1_pt[2].x + loArm1_pt[3].x) / 4;

      avgY = (loArm1_pt[0].y + loArm1_pt[1].y + loArm1_pt[2].y + loArm1_pt[3].y) / 4;

      avgZ = (loArm1_pt[0].z + loArm1_pt[1].z + loArm1_pt[2].z + loArm1_pt[3].z) / 4;

      Vector3DF loArmRotatePoint(avgX, avgY, avgZ);

      Vector3DF handRotatePoint(avgX, avgY, avgZ-(LOARM_H1-LOARM_H2));

      tempPt = handRotatePoint;

 

      Vector3DF rotatePhiAxis(cosf(elbow_theta), sinf(elbow_theta), 0);

      Vector3DF rotatePsiAxis = (upArm1_pt[4] - upArm1_pt[0]).Normalize();

 

      for (int i = 0; i < 8; i++)

      {

            loArm1_pt[i]      =           (loArmRotatePoint-orig_loArm1_avg) + orig_loArm1_pt[i];

            hand1_pt[i]       =           (handRotatePoint-orig_hand1_avg) + orig_hand1_pt[i];

 

            RotateAroundPoint(loArm1_pt[i], loArmRotatePoint, phi, rotatePhiAxis);

            RotateAroundPoint(hand1_pt[i], loArmRotatePoint, phi, rotatePhiAxis);        

      }

 

      for (int i = 0; i < 8; i++)

      {

            RotateAroundPoint(loArm1_pt[i], loArmRotatePoint, psi, rotatePsiAxis);

            RotateAroundPoint(hand1_pt[i], loArmRotatePoint, psi, rotatePsiAxis);

      }

 

 

      wrist_phi = phi;

      wrist_psi = psi;

}

 

void Body::Reset()

{

      RotateElbow(0,0);

      RotateWrist(0,0);

}

 

A.2.4 Point.hpp

 

 

#ifndef __POINT_HPP__

#define __POINT_HPP__

 

 

#include "gamex.hpp"

 

#define ZERO_G          187

#define G_OFFSET  221

#define PI              3.14159f

#define OFFSET          Vector3DF( 748, 748, 748 )

 

class Point

{

protected:

      Vector3DF offset;                         // offset values of accelerometer

      Vector3DF gravity;                              // Gravity directional vector

      ImageX image;                                   // Image of a single "Dot"

      CameraX *cam;

      Point *parent;

      int dist;

 

      int prev_x;

 

      void ForceMove(void);

      void LPFilter( Vector3DF &a );

      void HPFilter( Vector3DF &v );

      float NormalizeAngle(float angle);

 

      float round( float a );

 

 

public:

      Point();

      Point(Vector3DF _pos, int _dist, Point *_parent, CameraX *_cam);

      void Run( Vector3DF t, CameraX *c );

 

      Vector3DF pos;

      Vector3DF vel;

      Vector3DF acc;

 

      float phi, theta, psi;

 

      void Draw();

      void Reset();

};

 

 

 

#endif

 

A.2.5 Point.cpp

 

#include "Point.hpp"

 

Point::Point() {

}

 

Point::Point(Vector3DF _pos, int _dist, Point *_parent, CameraX *_cam){

 

      // Create the image of a Dot:

      image.Create (128, 128, true); // 128x128 pixels, with transparency

      for (int px = 0 ; px < 128 ; px++)

            for (int py = 0 ; py < 128 ; py++)

                  if (64.0f > sqrtf ((float) ((px-64)*(px-64)+(py-64)*(py-64))))

                        image.SetPixel (px,py, px,py,px+py,192+py-px);

 

      // initialize position, velocity, acceleration vectors

      //pos.Set( 0.0f, 0.0f, 0.0f );

      pos = _pos;

      vel.Set( 0.0f, 0.0f, 0.0f );

      acc.Set( 0.0f, 0.0f, 0.0f );

 

      phi = 0;

      theta = 0;

      psi = 0;

 

      // initialize gravity vector to be initially up

      gravity.Set( ZERO_G, ZERO_G, ZERO_G );//G_OFFSET );

 

      prev_x = ZERO_G;

 

      if (_parent == NULL)

            parent = this;

      else

            parent = _parent;

      cam = _cam;

      dist = _dist;

}

 

void Point::Run( Vector3DF t, CameraX *c ){

 

      // grab cam object for projection view

      //cam = c[0];

 

      // update the acceleration vector based on input data

      //    and filter it

      acc = (t-gravity);

 

      // kill x-axis g-field acceleration

      //float gx = pow( pow(36,2)-pow(acc.y,2)-pow(acc.z,2), .5 );

 

 

      //float gx = 34 - pow( pow(acc.y,2)+pow(acc.z,2), .5 );

      //vel.x = gx;

 

      // compute angles and delta angles

      phi = atan2( acc.y, acc.z );        // phi = aten( y/z )

      theta = atan2( acc.z, acc.x );

 

      //vel.x = round( 33*cos(theta) );

      float gx = 33*cos(theta);

      vel.x = gx;

      acc.x = acc.x-gx;

 

      LPFilter( acc );

 

      psi += acc.x;

 

      // update position based upon angles

      pos.x = dist*cos( psi );

      pos.y = dist*cos( phi ) + dist*sin( psi );;

      pos.z = dist*sin( phi );

 

      Vector3DF d = pos - parent->pos;

      pos = parent->pos + d*(dist/d.Length());

 

      // move point if clicked on

      //ForceMove();

 

}

 

/* LPFs the acceleration vecotr */

void Point::LPFilter( Vector3DF &a ){

 

      if( abs(a.x) < 0 )

            a.x = 0;

      if( abs(a.y) < 2 )

            a.y = 0;

      if( abs(a.z) < 2 )

            a.z = 0;

}

 

float Point::round( float a ){

 

      // get decimal part

      //int b = a;

      //float decimal = a-(float)b;

 

      //if( abs(decimal) >= .5 )

      //    return ceil( a );

      //else

      //    return floor( a );

 

      if( a < 0 )

            return floor( a );

      else

            return ceil(a);

 

}

 

float Point::NormalizeAngle(float angle)

{

      float result = angle;

 

      if (angle > 2*PI)

            result = angle - 2*PI*((int)(0.5*angle/PI));

      else if (angle < 0)

            result = angle - 2*PI*((int)(0.5*angle/PI)-1);

 

      return result;

}

 

/* HPFs the velocity vecotr */

void Point::HPFilter( Vector3DF &v ){

 

      if( v.x > 20 )

            v.x = 20;

      else if( v.x < -20 )

            v.x = -20;

 

      if( v.y > 20 )

            v.y = 20;

      else if( v.y < -20 )

            v.y = -20;

 

      if( v.z > 20 )

            v.z = 20;

      else if( v.z < -20 )

            v.z = -20;

}

 

void Point::ForceMove(){

     

      // temporary variables

      bool moved_point = false;

      int mousex = GameX.GetMouseX();

      int mousey = GameX.GetMouseY();

      float mousedx = (float)GameX.GetMouseDX();

      float mousedy = (float)GameX.GetMouseDY();

      float x,y,z;

 

      // move dot if clicked on

      if( GameX.IsMouseDown(MOUSE_LEFT) ){

 

            // find p1 on screen space

            cam->Project( pos, x, y, z );

 

            // if within space and no previous point has been moved, move this point

            if( ( pow(x-mousex,2)+pow(y-mousey,2) ) <= 60 && !moved_point){   // p1 and mouse are close to each other

 

                  moved_point = true;

 

                  Vector3DF o = cam->GetDirVector();        // outward vector

                  Vector3DF r = cam->GetSideVector();       // vector pointing to the right

                  Vector3DF u = r; u.Cross(o);              // vector pointing up

 

                  pos += ( r * Vector3DF(mousedx/2,mousedx/2,mousedx/2) );

                  pos -= ( u * Vector3DF(mousedy/2,mousedy/2,mousedy/2) );

            }

      }

}

 

void Point::Draw(){

 

      // Draw the arm

      GameX.SetDrawScale (1.0f);

      GameX.DrawLine3D( ColorX(150,150,150), pos, parent->pos );

 

      // display the point

      GameX.SetDrawScale (5.0f);

      GameX.DrawImage3D ( &image, pos );

 

      // display the acceleration vector

      //GameX.DrawLine3D( ColorX(255,0,0), pos, acc+pos );

 

      // display the velocity vector

      //GameX.DrawLine3D( ColorX(0,255,0), pos, vel+pos );

 

}

 

void Point::Reset()

{

      pos.Set(0,0,0);

      vel.Set(0,0,0);

      acc.Set(0,0,0);

      phi = 0;

      theta = 0;

      psi = 0;

      prev_x = 0;

}

 

A.2.6 Wrist.hpp

 

#include "Point.hpp"

 

#define NUM_SAMPLES 4

 

class Wrist : public Point

{

public:

      Wrist();

      Wrist(Vector3DF _pos, int _dist, Point *_parent, CameraX *_cam);

      void Run( Vector3DF t );

      void Draw();

      float x_avg;

      void Reset();

 

private:

      float avg_vector[NUM_SAMPLES];

      int oldest_ptr;

     

      int zero_cnt;

 

      void filter_X(void);

 

      bool vel_up;

      bool vel_down;

     

};

 

A.2.7 Wrist.cpp

 

#include "Wrist.hpp"

#include "Console.hpp"

 

extern Console *console;

 

 

float yn_1, xn_1, R;

Wrist::Wrist() : Point()

{

}

 

Wrist::Wrist(Vector3DF _pos, int _dist, Point *_parent, CameraX *_cam) : Point (_pos, _dist, _parent, _cam)

{

      //offset.Set( 189, 190, 195 );

      offset.Set( 187, 187, 187 );

      gravity.Set( 31.5, 32, 30.5 );

 

      // set all initial samples to zero

      for( int i=0; i<NUM_SAMPLES; i++ )

            avg_vector[i] = 0;

 

      oldest_ptr = 0;

      x_avg = 0;

      zero_cnt = 0;

      yn_1 = 0;

      xn_1 = 0;

      R = .1;

      dist = 120;

 

      vel_up = false;

      vel_down = false;

     

}

 

 

 

void Wrist::Run( Vector3DF rawAcc )

{

      // Get the acceleration values by subtracting the DC offset

      acc = (rawAcc-OFFSET);

 

      // compute angles

      phi = atan2( acc.y, acc.z );

      theta = atan2( acc.z, acc.x );

 

      // lowpass filter the acceleration

      LPFilter( acc );

 

      // filter x-axis acc further for numerical integration

      filter_X();

 

      //psi += acc.x;

 

      // update position based upon angles

      pos.x = dist*cos( psi-PI/2 );

      pos.y = dist*cos( phi ) + dist*sin( psi );

      pos.z = dist*sin( phi );

 

      // set your position based upon your parent's

      pos = parent->pos + pos;

 

      // make sure you're always d away from your parent

      Vector3DF d = pos - parent->pos;

      pos = parent->pos + d*(dist/d.Length());

 

}

 

 

 

void Wrist::filter_X(){

 

      //---------- Do x-axis DC-blocker ----------//

      // implement y[n] = x[n] - x[n-1] + R*y[n-1]

      float temp = acc.x;           // x[n] value

      acc.x = temp - xn_1 + R*yn_1;

 

      // update y[n-1],x[n-1]

      yn_1 = acc.x;

      xn_1 = temp;

      //--------- End DC blocker ----------//

 

      //---------- Do Moving average filter on acceleration ----------//

      // update the avg acceleration

      //  add the newest sample, subtract the oldest sample

      x_avg -= avg_vector[oldest_ptr]/NUM_SAMPLES;

      avg_vector[oldest_ptr] = acc.x;

      x_avg += acc.x/NUM_SAMPLES;

 

      // put the newest sample in the oldest sample spot

      avg_vector[oldest_ptr] = acc.x;

 

      // the next element is now the oldest sample

      oldest_ptr++;

      if( oldest_ptr == NUM_SAMPLES )

            oldest_ptr = 0;

      //---------- End Filtering acceleration ----------//

 

      //---------- Magnitude shape the velocity and Do Numerical Integration ----------//

      if( x_avg >= 0.f )

            vel.x += log( x_avg+1 )/10;

      if( x_avg < 0.0f )

            vel.x -= log( -1*x_avg + 1 )/10;

 

      if( -0.01f < vel.x && vel.x < 0.01f )

            vel.x = 0;

      //---------- End Magnitude shape velocity, Num Integration ----------//

 

      //---------- Damp the velocity ----------//

      if( x_avg > -.4 && x_avg < .4 ){

            vel.x *= .1;

            zero_cnt++;

      }

      //---------- End Velocity damping ----------//

 

      //---------- Debounce velocity ----------//

      if( vel.x < -.5f && !vel_up ){

            vel_down = true;

      }else if( vel.x > .5f && !vel_down ){

            vel_up = true;

      }else if( zero_cnt >= 3 ){

            vel_up = false;

            vel_down = false;

            zero_cnt = 0;

      }

 

      if( vel.x > 0 && vel_down )

            vel.x = 0;

      if( vel.x < 0 && vel_up )

            vel.x = 0;

 

 

 

      // ---------- numerically integrate the velocity to extract angular position ----------//

      //pos.x += vel.x;

      psi += vel.x/dist;

 

      // put bounds on the angle

      if( psi <= -PI/2 )

            psi = -PI/2;

 

      if( psi >= PI/2 )

            psi = PI/2;

}

 

void Wrist::Draw()

{

      // Draw the arm

      GameX.DrawLine3D( ColorX(150,150,150), pos, parent->pos );

 

      // display the point

      GameX.DrawImage3D ( &image, pos );

 

      // display wrist orientation

      GameX.DrawLine3D( ColorX(255,0,0), pos, Vector3DF( pos.x-10*cos(theta), pos.y, pos.z+10*sin(theta) ) );

 

}

 

//---------- Set everything back to zero ----------//

void Wrist::Reset()

{

      Point::Reset();

      x_avg = 0;

      zero_cnt = 0;

      oldest_ptr = 0;

      for (int i = 0; i < NUM_SAMPLES; i++)

            avg_vector[i] = 0;

}

 

A.2.8 Elbow.hpp

 

 

#include "Point.hpp"

 

 

class Elbow : public Point

{

public:

      Elbow();

      Elbow(Vector3DF _pos, int _dist, Point *_parent, CameraX *_cam);

      void Run( Vector3DF t );

      void Draw();

 

private:

     

     

};

 

A.2.9 Elbow.cpp

 

#include "Elbow.hpp"

 

 

Elbow::Elbow()

{

}

 

Elbow::Elbow(Vector3DF _pos, int _dist, Point *_parent, CameraX *_cam) : Point (_pos, _dist, _parent, _cam)

{

      offset.Set( 187, 187, 187 );

      pos = _pos;

      dist = _dist;

      parent = _parent;

      cam = _cam;

 

}

 

void Elbow::Run( Vector3DF rawAcc )

{

      Vector3DF prev = pos;

      acc = rawAcc - OFFSET;

      LPFilter( acc );

 

      float hyp = acc.Length();

 

      phi = atan2( acc.y, acc.z );

      theta = asin( acc.x/hyp );

 

      if( theta > 45 )

            phi = asin( acc.y/hyp );

 

      vel.x = dist*sin( theta );

      vel.y = dist*cos( phi );

      vel.z = dist*sin( phi );//+ dist*cos(theta);

 

      pos = parent->pos + vel;

 

      pos.x = vel.x;

 

      phi *= (180/PI);

      theta *= (180/PI);

}

 

void Elbow::Draw()

{

      // Draw the arm

      GameX.SetDrawScale (1.0f);

      GameX.DrawLine3D( ColorX(150,150,150), pos, parent->pos );

 

      // display the point

      GameX.SetDrawScale (5.0f);

      GameX.DrawImage3D ( &image, pos );

 

      // display the acceleration vector

      //GameX.DrawLine3D( ColorX(255,0,0), pos, acc+pos );

}

 

A.2.10 SerialPort.hpp

 

#ifndef SERIAL_PORT_HPP

#define SERIAL_PORT_HPP

 

#include <string>

#include <tchar.h>

#include <windows.h>

 

using namespace std;

 

class SerialPort

{

private:

      HANDLE hSerialPort;

public:

      SerialPort();

      ~SerialPort();

 

      HRESULT Init(string portName);

      HRESULT Read(BYTE* buffer, DWORD* BytesRead);

      HRESULT LastError(char* buffer);

};

 

 

#endif

 

A.2.11 SerialPort.cpp

 

#include "SerialPort.hpp"

 

SerialPort::SerialPort()

{

}

 

SerialPort::~SerialPort()

{

      if(hSerialPort != INVALID_HANDLE_VALUE)

            CloseHandle(hSerialPort);

}

 

HRESULT SerialPort::Init(string portName)

{

      DWORD Success;

      DCB MyDCB = {0};

      LPCOMMTIMEOUTS MyCommTimeouts = (LPCOMMTIMEOUTS) malloc(sizeof(LPCOMMTIMEOUTS));

      HRESULT result = S_OK;

 

      try

      {

            hSerialPort = ::CreateFile(portName.c_str(),

                  GENERIC_READ | GENERIC_WRITE, 0, 0,

                  OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);

 

            if ( hSerialPort == INVALID_HANDLE_VALUE )

                  return E_FAIL;

 

            Success = GetCommState(hSerialPort, &MyDCB);

            if (!Success)

                  return E_FAIL;

 

            MyDCB.DCBlength = sizeof(DCB);

            MyDCB.BaudRate = 9600;

            MyDCB.ByteSize = 8;

            MyDCB.Parity = NOPARITY;

            MyDCB.StopBits = ONESTOPBIT;

            MyDCB.fDsrSensitivity = 0;

            MyDCB.fDtrControl = DTR_CONTROL_ENABLE;

            MyDCB.fOutxDsrFlow = 0;

            MyDCB.ByteSize = 8;

            MyDCB.EofChar = 0;

            MyDCB.ErrorChar = 0;

            MyDCB.EvtChar = 0;

            MyDCB.fAbortOnError = 0;

            MyDCB.fBinary = 1;

            MyDCB.fDsrSensitivity = 0;

            MyDCB.fDummy2 = 0;

            MyDCB.fInX = 0;

            MyDCB.fNull = 0;

            MyDCB.fOutX = 0;

            MyDCB.fOutxCtsFlow = 1;

            MyDCB.fOutxDsrFlow = 0;

            MyDCB.wReserved = 0;

            MyDCB.wReserved1 = 0;

            MyDCB.XoffChar = 19;

            MyDCB.XoffLim = 512;

            MyDCB.XonChar = 17;

            MyDCB.XonLim = 2048;

 

            Success = SetCommState(hSerialPort, &MyDCB);

            if (!Success)

                  return E_FAIL;

 

            Success = GetCommTimeouts(hSerialPort, MyCommTimeouts);

            if (!Success)

                  return E_FAIL;

 

            MyCommTimeouts->ReadIntervalTimeout = MAXDWORD;

            MyCommTimeouts->ReadTotalTimeoutConstant = 1;

            MyCommTimeouts->ReadTotalTimeoutMultiplier = MAXDWORD;

            MyCommTimeouts->WriteTotalTimeoutConstant = 0;

            MyCommTimeouts->WriteTotalTimeoutMultiplier = 0;

 

            Success = SetCommTimeouts(hSerialPort, MyCommTimeouts);

            if (!Success)

                  return E_FAIL;

      }

      catch(...)

      {

            result = E_FAIL;

      }

 

      return result;

}

 

HRESULT SerialPort::Read(BYTE* buffer, DWORD* BytesRead)

{

      DWORD Success;

      HRESULT result = S_OK;

 

      try

      {

            Success = ReadFile(hSerialPort, buffer, 255, BytesRead, NULL);

            if (!Success)

                  result = E_FAIL;

      }

      catch(...)

      {

            result = E_FAIL;

      }

      return result;

}

 

HRESULT SerialPort::LastError(char* buffer)

{

      /*

      FormatMessage(

            FORMAT_MESSAGE_ALLOCATE_BUFFER |

            FORMAT_MESSAGE_FROM_SYSTEM |

            FORMAT_MESSAGE_IGNORE_INSERTS,

            NULL,

            GetLastError(),

            MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), // Default language

            (LPTSTR) buffer,

            0,

            NULL

      );

*/

      sprintf( buffer, "%d", GetLastError() );

      return S_OK;

}

 

A.2.12 LogFile.hpp

 

#include <windows.h>

#include <stdio.h>

#include <assert.h>

 

class LogFile

{

private:

      FILE *accLogFile;

      bool writeEn, doneReadingLog;

public:

      LogFile();

      ~LogFile()

      {

            if (accLogFile != NULL)

            {

                  fprintf(accLogFile, "\n");

                  fclose(accLogFile);

            }

      };

 

      bool OpenForWriting(char* fileName);

      bool OpenForReading(char* fileName);

      void Close();

 

      bool Printf(LPCSTR lpszFormat, ...);

      int ReadLine(BYTE buffer[]);

      bool IsLogFinished() {return doneReadingLog;};

};

 

A.2.13 LogFile.cpp

 

 

#include "LogFile.hpp"

 

LogFile::LogFile()

{

      writeEn = false;

      doneReadingLog = false;

      accLogFile = NULL;

}

 

bool LogFile::OpenForWriting(char* fileName)

{

      Close();

      accLogFile = fopen(fileName, "w+");

 

      if (accLogFile == NULL)

            return false;

      else

      {

            writeEn = true;

            return true;

      }

}

 

bool LogFile::OpenForReading(char* fileName)

{

      Close();

      accLogFile = fopen(fileName, "r");

 

      if (accLogFile == NULL)

            return false;

      else

      {

            doneReadingLog = false;

            return true;

      }

}

 

void LogFile::Close()

{

      doneReadingLog = false;

      if (accLogFile != NULL)

            fclose(accLogFile);

}

 

bool LogFile::Printf(LPCSTR lpszFormat, ...)

{

      if (accLogFile == NULL)

            return false;

 

      try

      {

            va_list args;

            va_start(args, lpszFormat);

           

            int nBuf;

            char szBuffer[8024];

           

            nBuf = _vsnprintf(szBuffer, sizeof(szBuffer), lpszFormat, args);

           

            assert(nBuf < sizeof(szBuffer));  //Output truncated as it was > sizeof(szBuffer)

           

            fprintf(accLogFile, szBuffer);

 

            va_end(args);

      }

      catch(...)

      {

            return false;

      }

 

      return true;

}

 

int LogFile::ReadLine(BYTE buffer[])

{

      int index = 0;

      char line[128];

      char* rv;

 

      if (doneReadingLog)

            return 0;

 

      if ( (rv = fgets(line, 128, accLogFile)) != NULL )

      {

            if (line[0] == '\n')

                  doneReadingLog = true;

            else

            {

                  line[strlen(line)-1] = '\0';

 

                  int BytesRead, tempInt;

                  char *nextByte, *tempLine = &line[0];

 

                  for (index = 0; ; index++)

                  {

                        BytesRead = sscanf(tempLine, "%d", &tempInt);

                        if (BytesRead == 1)

                              buffer[index] = tempInt;

                        else

                              break;

 

                        nextByte = strstr(tempLine, " ");

                        if (nextByte == NULL)

                              break;

                        else

                              tempLine = nextByte+1;

                  }

 

                  return index;

            }

      }

      else

            doneReadingLog = true;

 

      return 0;

}

 

A.2.14 Graph.hpp

 

 

#ifndef GRAPH_HPP

#define GRAPH_HPP

 

#include <string>

using namespace std;

 

class Graph

{

private:

      float *ptBuffer;

      int *ptScreenBuffer;

      int numPoints;

      int startIndex;

      int nextIndex;

      int x1,y1,x2,y2,ymag,xmid,ymid;

      int drawDeltaX;

      float yaxisMin, yaxisMax, yaxisRange;

      float maxSeen, minSeen;

      bool showGraph, timeout;

      char txt_title[20], txt_ymax[10], txt_ymin[10], txt_xmax[10], txt_minmaxSeen[40];

public:

      Graph(string _title, float _yaxisMin, float _yaxisMax, int _x1, int _y1, int _x2, int _y2);

      ~Graph();

      void SetSize(int _x1, int _y1, int _x2, int _y2);

      void AddPoint(float pt);

      void ClearAll();

      void Draw();

      void Show() {showGraph = true;};

      void Hide() {showGraph = false;};

      void Toggle() {showGraph = !showGraph;};

      void SetShowGraph(bool _showGraph) {showGraph = _showGraph;};

};

 

#endif

 

A.2.15 Graph.cpp

 

 

#include "Graph.hpp"

#include "gamex.hpp"

 

Graph::Graph(string _title, float _yaxisMin, float _yaxisMax, int _x1, int _y1, int _x2, int _y2)

{

      SetSize(_x1+10, _y1+10, _x2-10, _y2-10);

      numPoints = x2-x1;

      yaxisMin = _yaxisMin;

      yaxisMax = _yaxisMax;

      yaxisRange = yaxisMax - yaxisMin;

 

      minSeen = yaxisMax;

      maxSeen = yaxisMin;

      sprintf(txt_minmaxSeen, "min=%3.1f   max=%3.1f", minSeen, maxSeen);

 

      sprintf(txt_ymin, "%3.1f", yaxisMin);

      sprintf(txt_ymax, "%3.1f", yaxisMax);

      sprintf(txt_xmax, "%d", numPoints);

      sprintf(txt_title, "%s", _title.c_str());

 

      startIndex = 0;

      nextIndex = 0;

      showGraph = false;

      drawDeltaX = 1;

      timeout = false;

      ptBuffer = (float*) malloc(numPoints*sizeof(float));

      ptScreenBuffer = (int*) malloc(numPoints*sizeof(int));

}

 

Graph::~Graph()

{

      free(ptBuffer);

}

 

void Graph::SetSize(int _x1, int _y1, int _x2, int _y2)

{

      x1 = _x1;

      y1 = _y1;

      x2 = _x2;

      y2 = _y2;

      ymag = y2-y1;

      xmid = x1+(x2-x1)/2;

      ymid = y1+(y2-y1)/2;

};

 

void Graph::AddPoint(float pt)

{

      ptBuffer[nextIndex] = pt;

 

      if (!timeout)

      {

            if (nextIndex > 60)

                  timeout = true;

      }

 

      int ypos = (int) (y1 + ((yaxisMax - pt) / yaxisRange) * ymag);

      if (ypos < y1)

            ptScreenBuffer[nextIndex] = y1;

      else if (ypos > y2)

            ptScreenBuffer[nextIndex] = y2-1;

      else

      {

            ptScreenBuffer[nextIndex] = ypos;

            if (pt > maxSeen && timeout)

            {

                  maxSeen = pt;

                  sprintf(txt_minmaxSeen, "min=%3.1f   max=%3.1f", minSeen, maxSeen);

            }

            if (pt < minSeen && timeout)

            {

                  minSeen = pt;

                  sprintf(txt_minmaxSeen, "min=%3.1f   max=%3.1f", minSeen, maxSeen);

            }

      }

      nextIndex = (nextIndex + 1) % numPoints;

      if (nextIndex == startIndex)

            startIndex = (startIndex + 1) % numPoints;

}

 

void Graph::ClearAll()

{

      startIndex = 0;

      nextIndex = 0;

}

 

void Graph::Draw()

{

      if (showGraph)

      {

            // Draw Points

            int prevX = x1;

            int prevY = ptScreenBuffer[startIndex];

            for (int i = (startIndex+1)%numPoints; i != nextIndex; i = (i+1)%numPoints, prevX++)

            {

                  GameX.DrawLine(ColorX(0,255,0), prevX, prevY, prevX+1, ptScreenBuffer[i]);

                  prevY =  ptScreenBuffer[i];

            }

 

            GameX.DrawText(xmid-40, y1+5, txt_minmaxSeen);

      }

      else

            GameX.DrawText(xmid-10, ymid, "DISABLED");

 

      // Draw Axis

      GameX.DrawLine(ColorX(0,0,255), x1, y2, x2, y2);            // x-axis

      GameX.DrawLine(ColorX(0,0,255), x1, y1, x1, y2);            // y-axis

 

      GameX.DrawText(x1-10, y1, txt_ymax);

      GameX.DrawText(x1-10, y2-12, txt_ymin);

      GameX.DrawText(x2-10, y2-10, txt_xmax);

      GameX.DrawText(xmid-10, y1-10, txt_title);

}

 

A.2.16 Console.hpp

 

#include <list>

#include <string>

#include <assert.h>

#include <windows.h>

#include <process.h>

using namespace std;

 

typedef list<string> LISTSTR;

 

class Console

{

private:

      int x1, x2, y1, y2, maxLines, maxChars;

      int locate_x, locate_y;

      bool showConsole;

      LISTSTR strBuffer;

      void NewLine();

      void _Print(string newString);

      FILE *logFile;

      HANDLE hBufferMutex;

      DWORD dwWaitResult;

public:

      Console();

      Console(char* _logFileName, int x1, int y1, int x2, int y2);

      ~Console();

      void SetSize(int x1, int y1, int x2, int y2);

      void Printf(LPCSTR lpszFormat, ...);

      void _Printf(string newString);

      void Printfn(LPCSTR lpszFormat, ...);

      void _Printfn(string newString);

      void Show() {showConsole = true;};

      void Hide() {showConsole = false;};

      void Toggle() {showConsole = !showConsole;};

      void Draw();

};

 

A.2.17 Console.cpp

 

#include "Console.hpp"

#include "gamex.hpp"

 

Console::Console(char* _logFileName, int _x1, int _y1, int _x2, int _y2)

{

      SetSize(_x1, _y1, _x2, _y2);

      showConsole = false;

      logFile = fopen(_logFileName, "w+");

      //hBufferMutex = CreateMutex( NULL, FALSE, "console buffer mutex" );

}

 

Console::~Console()

{

      fclose(logFile);

}

 

void Console::SetSize(int _x1, int _y1, int _x2, int _y2)

{

      string tempStr = "";

 

      //dwWaitResult = WaitForSingleObject(hBufferMutex, INFINITE);

      strBuffer.push_back(tempStr);

      //ReleaseMutex(hBufferMutex);

 

      x1 = _x1;

      y1 = _y1;

      x2 = _x2;

      y2 = _y2;

      locate_x = 0;

      locate_y = 0;

      maxLines = (y2-y1)/15;

      maxChars = (int)((float)(x2-x1)/7.7);

}

 

void Console::NewLine()

{

      //dwWaitResult = WaitForSingleObject(hBufferMutex, INFINITE);

 

      string tempStr = "";

      strBuffer.push_back(tempStr);

 

      if ((int) strBuffer.size() > maxLines)

            strBuffer.pop_front();

 

      //ReleaseMutex(hBufferMutex);

}

 

void Console::_Print(string newString)

{

      string stringRem = newString;

      int charsRem = (int) stringRem.size();

      //dwWaitResult = WaitForSingleObject(hBufferMutex, INFINITE);

      int curLineSpaces = maxChars - (int) (strBuffer.back().size());

      //ReleaseMutex(hBufferMutex);

 

      while ( charsRem > 0 )

      {

            if ( curLineSpaces >= charsRem )

            {

                  //dwWaitResult = WaitForSingleObject(hBufferMutex, INFINITE);

                  strBuffer.back() += stringRem;

                  //ReleaseMutex(hBufferMutex);

                  break;

            }

            else

            {

                  //dwWaitResult = WaitForSingleObject(hBufferMutex, INFINITE);

                  strBuffer.back() += stringRem.substr(0, curLineSpaces);

                  //ReleaseMutex(hBufferMutex);

                  stringRem = stringRem.substr(curLineSpaces+1, (int)stringRem.size());

                  charsRem = (int) stringRem.size();

 

                  curLineSpaces = maxChars;

                  NewLine();

            }

      }

}

 

void Console::_Printf(string newString)

{

      int curPos = 0, delimPos = 0;

      int numChars = (int)newString.size();

      string addString = "";

 

      while(curPos < numChars)

      {

            delimPos = (int) newString.find("\n", curPos);

            if (delimPos == -1)

            {

                  _Print(newString.substr(curPos, numChars-curPos));

                  break;

            }

            else

            {

                  string pString = newString.substr(curPos, delimPos-curPos);

                  _Print(newString.substr(curPos, delimPos-curPos));

                  curPos = delimPos + 1;

                  NewLine();

            }

      }

}

 

void Console::Printf(LPCSTR lpszFormat, ...)

{

      try

      {

            va_list args;

            va_start(args, lpszFormat);

           

            int nBuf;

            char szBuffer[8024];

           

            nBuf = _vsnprintf(szBuffer, sizeof(szBuffer), lpszFormat, args);

           

            assert(nBuf < sizeof(szBuffer));  //Output truncated as it was > sizeof(szBuffer)

           

            _Printf(szBuffer);

 

            va_end(args);

      }

      catch(...)

      {

            assert(0);

      }

}

 

void Console::_Printfn(string newString)

{

      _Printf(newString);

      NewLine();

}

 

void Console::Printfn(LPCSTR lpszFormat, ...)

{

      try

      {

            va_list args;

            va_start(args, lpszFormat);

           

            int nBuf;

            char szBuffer[8024];

           

            nBuf = _vsnprintf(szBuffer, sizeof(szBuffer), lpszFormat, args);

           

            assert(nBuf < sizeof(szBuffer));  //Output truncated as it was > sizeof(szBuffer)

 

            fprintf(logFile, "%s\n", szBuffer);

           

            _Printf(szBuffer);

 

            va_end(args);

      }

      catch(...)

      {

            assert(0);

      }

 

      NewLine();

}

 

void Console::Draw()

{

      if (!showConsole)

            return;

 

      int i = 0;

      char *temp;

      LISTSTR::iterator ptr;

 

      // Draw Window

      GameX.SetDrawMode (DRAW_GHOST);

      GameX.DrawRect(ColorX(185,223,251), x1, y1, x2, y2);

 

      // Draw Text

      //dwWaitResult = WaitForSingleObject(hBufferMutex, INFINITE);

      for (ptr =  strBuffer.begin(); ptr != strBuffer.end(); ++ptr, i++)

      {

            temp = (char*)(ptr->c_str());

            GameX.DrawText( x1+10, y1+10+i*15, temp );

      }

      //ReleaseMutex(hBufferMutex);

}

 

A.2.18 Button.hpp

 

 

#ifndef __BUTTON_HPP__

#define __BUTTON_HPP__

 

class Button

{

private:

      int x1, y1, x2, y2;

      bool mousePressed, disabled;

      char buttonText[24];

      int strLen;

public:

      Button(char* _buttonText, int x1, int y1, int x2, int y2);

      bool Run();

      void Draw();

      void Disable() {disabled = true;};

      void Enable() {disabled = false;};

      void Toggle() {disabled = !disabled;};

};

 

#endif

 

A.2.19 Button.cpp

 

 

#include "Body.hpp"

 

#define THICK           4           // Thickness of Body (y-axis)

 

#define UPARM_W1_1      18          // Top inner x-cord

#define UPARM_W1_2      25          // Top outer x-cord

#define UPARM_W2_1      18          // Bottom inner x-cord

#define UPARM_W2_2      25          // Bottom outer x-cord

#define UPARM_H1  50          // Top Height

#define UPARM_H2  15          // Bottom Height

#define UPARM_DIST      (UPARM_H1-UPARM_H2)

 

#define LOARM_W1_1      18          // Top inner x-cord

#define LOARM_W1_2      25          // Top outer x-cord

#define LOARM_W2_1      18          // Bottom inner x-cord

#define LOARM_W2_2      25          // Bottom outer x-cord

#define LOARM_H1  15          // Top Height

#define LOARM_H2  -10         // Bottom Height

#define LOARM_DIST      (LOARM_H1-LOARM_H2)

 

#define HAND_W1_1 18          // Top inner x-cord

#define HAND_W1_2 25          // Top outer x-cord

#define HAND_W2_1 18          // Bottom inner x-cord

#define HAND_W2_2 24          // Bottom outer x-cord

#define HAND_H1         -10         // Top Height

#define HAND_H2         -25         // Bottom Height

#define HAND_DIST (HAND_H1-HAND_H2)

#define HAND_THICK      7

 

#define SHO_W1          18          // Top x-cord

#define SHO_W2          18          // Bottom x-cord

#define SHO_H1          50          // Top Height

#define SHO_H2          40          // Bottom Height

 

#define TOR_W1          15          // Top x-cord

#define TOR_W2          12          // Bottom x-cord

#define TOR_H1          40          // Top Height

#define TOR_H2          0           // Bottom Height

 

#define PEL_W1          12          // Top x-cord

#define PEL_W2          12          // Bottom x-cord

#define PEL_H1          0           // Top Height

#define PEL_H2          -10         // Bottom Height

 

#define THI_W1_1  2           // Top inner x-cord

#define THI_W1_2  12          // Top outer x-cord

#define THI_W2_1  2           // Bottom inner x-cord

#define THI_W2_2  12          // Bottom outer x-cord

#define THI_H1          -10         // Top Height

#define THI_H2          -50         // Bottom Height

 

#define SHIN_W1_1 2           // Top inner x-cord

#define SHIN_W1_2 12          // Top outer x-cord

#define SHIN_W2_1 2           // Bottom inner x-cord

#define SHIN_W2_2 12          // Bottom outer x-cord

#define SHIN_H1         -50         // Top Height

#define SHIN_H2         -90         // Bottom Height

 

#define FEET_W1_1 2           // Top inner x-cord

#define FEET_W1_2 12          // Top outer x-cord

#define FEET_W2_1 2           // Bottom inner x-cord

#define FEET_W2_2 12          // Bottom outer x-cord

#define FEET_H1         -90         // Top Height

#define FEET_H2         -95         // Bottom Height

#define FEET_THICK      10

 

Body::Body()

{

      boxImg.Create(128,128,false);

      boxImg.Fill(150,150,150);

      for (int i=0; i < 128; i++)

      {

            boxImg.SetPixel(i, 0, 0, 0, 0);

            boxImg.SetPixel(i, 127, 0, 0, 0);

            boxImg.SetPixel(0, i, 0, 0, 0);

            boxImg.SetPixel(127, i, 0, 0, 0);

      }

 

      redImg.Create(128,128,false);

      redImg.Fill(255,0,0);

      for (int i=0; i < 128; i++)

      {

            redImg.SetPixel(i, 0, 0, 0, 0);

            redImg.SetPixel(i, 127, 0, 0, 0);

            redImg.SetPixel(0, i, 0, 0, 0);

            redImg.SetPixel(127, i, 0, 0, 0);

      }

 

      blueImg.Create(128,128,false);

      blueImg.Fill(0,0,255);

      for (int i=0; i < 128; i++)

      {

            blueImg.SetPixel(i, 0, 0, 0, 0);

            blueImg.SetPixel(i, 127, 0, 0, 0);

            blueImg.SetPixel(0, i, 0, 0, 0);

            blueImg.SetPixel(127, i, 0, 0, 0);

      }

 

      faceImg1.Create(128,128,false);

      faceImg1.Fill(250,250,0);

      for (int i=0; i < 128; i++)

      {

            faceImg1.SetPixel(i, 0, 0, 0, 0);

            faceImg1.SetPixel(i, 127, 0, 0, 0);

            faceImg1.SetPixel(0, i, 0, 0, 0);

            faceImg1.SetPixel(127, i, 0, 0, 0);

      }

 

      faceImg2.Load("Images\\Smiley.bmp",false);

      faceImg3.Load("Images\\NotSmiley.bmp",false);

 

      bigRedImg.Load("Images\\BigRed.bmp",false);

 

      elbow_phi = 0;

      elbow_theta = 0;

      wrist_phi = 0;

      wrist_psi = 0;

 

      // Feet

      fe1_pt[0].Set(-FEET_W1_2,-THICK-FEET_THICK, FEET_H1);

      fe1_pt[1].Set(-FEET_W1_1,-THICK-FEET_THICK, FEET_H1);

      fe1_pt[2].Set(-FEET_W1_2, THICK,                FEET_H1);

      fe1_pt[3].Set(-FEET_W1_1, THICK,                FEET_H1);

      fe1_pt[4].Set(-FEET_W2_2,-THICK-FEET_THICK, FEET_H2);

      fe1_pt[5].Set(-FEET_W2_1,-THICK-FEET_THICK, FEET_H2);

      fe1_pt[6].Set(-FEET_W2_2, THICK,                FEET_H2);

      fe1_pt[7].Set(-FEET_W2_1, THICK,                FEET_H2);

 

      fe2_pt[0].Set( FEET_W1_2,-THICK-FEET_THICK, FEET_H1);

      fe2_pt[1].Set( FEET_W1_1,-THICK-FEET_THICK, FEET_H1);

      fe2_pt[2].Set( FEET_W1_2, THICK,                FEET_H1);

      fe2_pt[3].Set( FEET_W1_1, THICK,                FEET_H1);

      fe2_pt[4].Set( FEET_W2_2,-THICK-FEET_THICK, FEET_H2);

      fe2_pt[5].Set( FEET_W2_1,-THICK-FEET_THICK, FEET_H2);

      fe2_pt[6].Set( FEET_W2_2, THICK,                FEET_H2);

      fe2_pt[7].Set( FEET_W2_1, THICK,                FEET_H2);

 

      // Shin

      sh1_pt[0].Set(-SHIN_W1_2,-THICK, SHIN_H1);

      sh1_pt[1].Set(-SHIN_W1_1,-THICK, SHIN_H1);

      sh1_pt[2].Set(-SHIN_W1_2, THICK, SHIN_H1);

      sh1_pt[3].Set(-SHIN_W1_1, THICK, SHIN_H1);

      sh1_pt[4].Set(-SHIN_W2_2,-THICK, SHIN_H2);

      sh1_pt[5].Set(-SHIN_W2_1,-THICK, SHIN_H2);

      sh1_pt[6].Set(-SHIN_W2_2, THICK, SHIN_H2);

      sh1_pt[7].Set(-SHIN_W2_1, THICK, SHIN_H2);

 

      sh2_pt[0].Set( SHIN_W1_1,-THICK, SHIN_H1);

      sh2_pt[1].Set( SHIN_W1_2,-THICK, SHIN_H1);

      sh2_pt[2].Set( SHIN_W1_1, THICK, SHIN_H1);

      sh2_pt[3].Set( SHIN_W1_2, THICK, SHIN_H1);

      sh2_pt[4].Set( SHIN_W2_1,-THICK, SHIN_H2);

      sh2_pt[5].Set( SHIN_W2_2,-THICK, SHIN_H2);

      sh2_pt[6].Set( SHIN_W2_1, THICK, SHIN_H2);

      sh2_pt[7].Set( SHIN_W2_2, THICK, SHIN_H2);

 

      // Thigh

      th1_pt[0].Set(-THI_W1_2,-THICK, THI_H1);

      th1_pt[1].Set(-THI_W1_1,-THICK, THI_H1);

      th1_pt[2].Set(-THI_W1_2, THICK, THI_H1);

      th1_pt[3].Set(-THI_W1_1, THICK, THI_H1);

      th1_pt[4].Set(-THI_W2_2,-THICK, THI_H2);

      th1_pt[5].Set(-THI_W2_1,-THICK, THI_H2);

      th1_pt[6].Set(-THI_W2_2, THICK, THI_H2);

      th1_pt[7].Set(-THI_W2_1, THICK, THI_H2);

 

      th2_pt[0].Set( THI_W1_1,-THICK, THI_H1);

      th2_pt[1].Set( THI_W1_2,-THICK, THI_H1);

      th2_pt[2].Set( THI_W1_1, THICK, THI_H1);

      th2_pt[3].Set( THI_W1_2, THICK, THI_H1);

      th2_pt[4].Set( THI_W2_1,-THICK, THI_H2);

      th2_pt[5].Set( THI_W2_2,-THICK, THI_H2);

      th2_pt[6].Set( THI_W2_1, THICK, THI_H2);

      th2_pt[7].Set( THI_W2_2, THICK, THI_H2);

 

      // Pelvis

      pel_pt[0].Set(-PEL_W1,-THICK, PEL_H1);

      pel_pt[1].Set( PEL_W1,-THICK, PEL_H1);

      pel_pt[2].Set(-PEL_W1, THICK, PEL_H1);

      pel_pt[3].Set( PEL_W1, THICK, PEL_H1);

      pel_pt[4].Set(-PEL_W2,-THICK, PEL_H2);

      pel_pt[5].Set( PEL_W2,-THICK, PEL_H2);

      pel_pt[6].Set(-PEL_W2, THICK, PEL_H2);

      pel_pt[7].Set( PEL_W2, THICK, PEL_H2);

 

      // Torso

      tor_pt[0].Set(-TOR_W1,-THICK, TOR_H1);

      tor_pt[1].Set( TOR_W1,-THICK, TOR_H1);

      tor_pt[2].Set(-TOR_W1, THICK, TOR_H1);

      tor_pt[3].Set( TOR_W1, THICK, TOR_H1);

      tor_pt[4].Set(-TOR_W2,-THICK, TOR_H2);

      tor_pt[5].Set( TOR_W2,-THICK, TOR_H2);

      tor_pt[6].Set(-TOR_W2, THICK, TOR_H2);

      tor_pt[7].Set( TOR_W2, THICK, TOR_H2);

 

      // Shoulder

      sho_pt[0].Set(-SHO_W1,-THICK, SHO_H1);

      sho_pt[1].Set( SHO_W1,-THICK, SHO_H1);

      sho_pt[2].Set(-SHO_W1, THICK, SHO_H1);

      sho_pt[3].Set( SHO_W1, THICK, SHO_H1);

      sho_pt[4].Set(-SHO_W2,-THICK, SHO_H2);

      sho_pt[5].Set( SHO_W2,-THICK, SHO_H2);

      sho_pt[6].Set(-SHO_W2, THICK, SHO_H2);

      sho_pt[7].Set( SHO_W2, THICK, SHO_H2);

 

      // Neck

      nec_pt[0].Set(-2,-2, 55);

      nec_pt[1].Set( 2,-2, 55);

      nec_pt[2].Set(-2, 2, 55);

      nec_pt[3].Set( 2, 2, 55);

      nec_pt[4].Set(-2,-2, 50);

      nec_pt[5].Set( 2,-2, 50);

      nec_pt[6].Set(-2, 2, 50);

      nec_pt[7].Set( 2, 2, 50);

 

      // Head

      hed_pt[0].Set(-10,-THICK, 80);

      hed_pt[1].Set( 10,-THICK, 80);

      hed_pt[2].Set(-10, THICK, 80);

      hed_pt[3].Set( 10, THICK, 80);

      hed_pt[4].Set(-7,-THICK, 55);

      hed_pt[5].Set( 7,-THICK, 55);

      hed_pt[6].Set(-7, THICK, 55);

      hed_pt[7].Set( 7, THICK, 55);

 

      // Arms

      upArm1_pt[0].Set(-UPARM_W1_2,-THICK, UPARM_H1);

      upArm1_pt[1].Set(-UPARM_W1_1,-THICK, UPARM_H1);

      upArm1_pt[2].Set(-UPARM_W1_2, THICK, UPARM_H1);

      upArm1_pt[3].Set(-UPARM_W1_1, THICK, UPARM_H1);

      upArm1_pt[4].Set(-UPARM_W2_2,-THICK, UPARM_H2);

      upArm1_pt[5].Set(-UPARM_W2_1,-THICK, UPARM_H2);

      upArm1_pt[6].Set(-UPARM_W2_2, THICK, UPARM_H2);

      upArm1_pt[7].Set(-UPARM_W2_1, THICK, UPARM_H2);

 

      upArm2_pt[0].Set( UPARM_W1_2,-THICK, UPARM_H1);

      upArm2_pt[1].Set( UPARM_W1_1,-THICK, UPARM_H1);

      upArm2_pt[2].Set( UPARM_W1_2, THICK, UPARM_H1);

      upArm2_pt[3].Set( UPARM_W1_1, THICK, UPARM_H1);

      upArm2_pt[4].Set( UPARM_W2_2,-THICK, UPARM_H2);

      upArm2_pt[5].Set( UPARM_W2_1,-THICK, UPARM_H2);

      upArm2_pt[6].Set( UPARM_W2_2, THICK, UPARM_H2);

      upArm2_pt[7].Set( UPARM_W2_1, THICK, UPARM_H2);

 

      loArm1_pt[0].Set(-LOARM_W1_2,-THICK, LOARM_H1);

      loArm1_pt[1].Set(-LOARM_W1_1,-THICK, LOARM_H1);

      loArm1_pt[2].Set(-LOARM_W1_2, THICK, LOARM_H1);

      loArm1_pt[3].Set(-LOARM_W1_1, THICK, LOARM_H1);

      loArm1_pt[4].Set(-LOARM_W2_2,-THICK, LOARM_H2);

      loArm1_pt[5].Set(-LOARM_W2_1,-THICK, LOARM_H2);

      loArm1_pt[6].Set(-LOARM_W2_2, THICK, LOARM_H2);

      loArm1_pt[7].Set(-LOARM_W2_1, THICK, LOARM_H2);

 

      loArm2_pt[0].Set( LOARM_W1_2,-THICK, LOARM_H1);

      loArm2_pt[1].Set( LOARM_W1_1,-THICK, LOARM_H1);

      loArm2_pt[2].Set( LOARM_W1_2, THICK, LOARM_H1);

      loArm2_pt[3].Set( LOARM_W1_1, THICK, LOARM_H1);

      loArm2_pt[4].Set( LOARM_W2_2,-THICK, LOARM_H2);

      loArm2_pt[5].Set( LOARM_W2_1,-THICK, LOARM_H2);

      loArm2_pt[6].Set( LOARM_W2_2, THICK, LOARM_H2);

      loArm2_pt[7].Set( LOARM_W2_1, THICK, LOARM_H2);

 

      hand1_pt[0].Set(-HAND_W1_2,-HAND_THICK, HAND_H1);

      hand1_pt[1].Set(-HAND_W1_1,-HAND_THICK, HAND_H1);

      hand1_pt[2].Set(-HAND_W1_2, HAND_THICK, HAND_H1);

      hand1_pt[3].Set(-HAND_W1_1, HAND_THICK, HAND_H1);

      hand1_pt[4].Set(-HAND_W2_2,-HAND_THICK, HAND_H2);

      hand1_pt[5].Set(-HAND_W2_1,-HAND_THICK, HAND_H2);

      hand1_pt[6].Set(-HAND_W2_2, HAND_THICK, HAND_H2);

      hand1_pt[7].Set(-HAND_W2_1, HAND_THICK, HAND_H2);

 

      hand2_pt[0].Set( HAND_W1_2,-HAND_THICK, HAND_H1);

      hand2_pt[1].Set( HAND_W1_1,-HAND_THICK, HAND_H1);

      hand2_pt[2].Set( HAND_W1_2, HAND_THICK, HAND_H1);

      hand2_pt[3].Set( HAND_W1_1, HAND_THICK, HAND_H1);

      hand2_pt[4].Set( HAND_W2_2,-HAND_THICK, HAND_H2);

      hand2_pt[5].Set( HAND_W2_1,-HAND_THICK, HAND_H2);

      hand2_pt[6].Set( HAND_W2_2, HAND_THICK, HAND_H2);

      hand2_pt[7].Set( HAND_W2_1, HAND_THICK, HAND_H2);

 

      for (int i = 0; i < 8; i++)

      {

            orig_fe1_pt[i] = fe1_pt[i];

            orig_fe2_pt[i] = fe2_pt[i];

            orig_sh1_pt[i] = sh1_pt[i];

            orig_sh2_pt[i] = sh2_pt[i];

            orig_th1_pt[i] = th1_pt[i];

            orig_th2_pt[i] = th2_pt[i];

            orig_pel_pt[i] = pel_pt[i];

            orig_tor_pt[i] = tor_pt[i];

            orig_sho_pt[i] = sho_pt[i];

            orig_nec_pt[i] = nec_pt[i];

            orig_hed_pt[i] = hed_pt[i];

            orig_upArm1_pt[i] = upArm1_pt[i];

            orig_upArm2_pt[i] = upArm2_pt[i];

            orig_loArm1_pt[i] = loArm1_pt[i];

            orig_loArm2_pt[i] = loArm2_pt[i];

            orig_hand1_pt[i] = hand1_pt[i];

            orig_hand2_pt[i] = hand2_pt[i];

      }

 

      float avgX, avgY, avgZ;

      avgX = (orig_loArm1_pt[0].x + orig_loArm1_pt[1].x + orig_loArm1_pt[2].x + orig_loArm1_pt[3].x) / 4;

      avgY = (orig_loArm1_pt[0].y + orig_loArm1_pt[1].y + orig_loArm1_pt[2].y + orig_loArm1_pt[3].y) / 4;

      avgZ = (orig_loArm1_pt[0].z + orig_loArm1_pt[1].z + orig_loArm1_pt[2].z + orig_loArm1_pt[3].z) / 4;

      orig_loArm1_avg.Set(avgX, avgY, avgZ);

 

      avgX = (orig_hand1_pt[0].x + orig_hand1_pt[1].x + orig_hand1_pt[2].x + orig_hand1_pt[3].x) / 4;

      avgY = (orig_hand1_pt[0].y + orig_hand1_pt[1].y + orig_hand1_pt[2].y + orig_hand1_pt[3].y) / 4;

      avgZ = (orig_hand1_pt[0].z + orig_hand1_pt[1].z + orig_hand1_pt[2].z + orig_hand1_pt[3].z) / 4;

      orig_hand1_avg.Set(avgX, avgY, avgZ);

 

      Reset();

 

}

 

void Body::Draw()

{

      // Feet

      DrawBoxPts(&boxImg, fe1_pt);

      DrawBoxPts(&boxImg, fe2_pt);

     

      // Shins

      DrawBoxPts(&boxImg, sh1_pt);

      DrawBoxPts(&boxImg, sh2_pt);

 

      // Thighs

      DrawBoxPts(&blueImg, th1_pt);

      DrawBoxPts(&blueImg, th2_pt);

 

      // Pelvis

      DrawBoxPts(&blueImg, pel_pt);

 

      // Torso

      //DrawBoxPts(&boxImg, tor_pt);

      DrawShirt(tor_pt);

 

      // Shoulder

      DrawBoxPts(&redImg, sho_pt);

 

      // Neck

      DrawBoxPts(&boxImg, nec_pt);

 

      // Arms

      DrawBoxPts(&redImg, upArm1_pt);

      DrawBoxPts(&redImg, upArm2_pt);

      DrawBoxPts(&redImg, loArm1_pt);

      DrawBoxPts(&redImg, loArm2_pt);

 

      // Hands

      DrawBoxPts(&boxImg, hand1_pt);

      DrawBoxPts(&boxImg, hand2_pt);

 

      // Head

      DrawFace(hed_pt);

 

      //GameX.DrawPoint3D(ColorX(255,255,255), tempPt);

      //GameX.DrawLine3D(ColorX(255,255,255), Vector3DF(50,0,50), Vector3DF(50,0,50)+ tempPt);

}

 

void Body::DrawBoxPts(Vector3DF pt1, Vector3DF pt2, Vector3DF pt3, Vector3DF pt4, Vector3DF pt5, Vector3DF pt6, Vector3DF pt7, Vector3DF pt8)

{

      GameX.DrawTexturedPolygon3D( &boxImg, pt3, pt4, pt1, pt2 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt5, pt6, pt7, pt8 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt1, pt2, pt5, pt6 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt2, pt4, pt6, pt8 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt4, pt3, pt8, pt7 );

      GameX.DrawTexturedPolygon3D( &boxImg, pt3, pt1, pt7, pt5 );

}

 

void Body::DrawBoxPts(ImageX *img, Vector3DF pt[])

{

      GameX.DrawTexturedPolygon3D( img, pt[2], pt[3], pt[0], pt[1] );

      GameX.DrawTexturedPolygon3D( img, pt[4], pt[5], pt[6], pt[7] );

      GameX.DrawTexturedPolygon3D( img, pt[0], pt[1], pt[4], pt[5] );

      GameX.DrawTexturedPolygon3D( img, pt[1], pt[3], pt[5], pt[7] );

      GameX.DrawTexturedPolygon3D( img, pt[3], pt[2], pt[7], pt[6] );

      GameX.DrawTexturedPolygon3D( img, pt[2], pt[0], pt[6], pt[4] );

}

 

void Body::DrawFace( Vector3DF pt[] )

{

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[2], pt[3], pt[0], pt[1] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[4], pt[5], pt[6], pt[7] );

      if (wrist_phi > elbow_phi)

            GameX.DrawTexturedPolygon3D( &faceImg3, pt[0], pt[1], pt[4], pt[5] );

      else

            GameX.DrawTexturedPolygon3D( &faceImg2, pt[0], pt[1], pt[4], pt[5] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[1], pt[3], pt[5], pt[7] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[3], pt[2], pt[7], pt[6] );

      GameX.DrawTexturedPolygon3D( &faceImg1, pt[2], pt[0], pt[6], pt[4] );

}

 

void Body::DrawShirt( Vector3DF pt[] )

{

      GameX.DrawTexturedPolygon3D( &redImg, pt[2], pt[3], pt[0], pt[1] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[4], pt[5], pt[6], pt[7] );

      GameX.DrawTexturedPolygon3D( &bigRedImg, pt[0], pt[1], pt[4], pt[5] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[1], pt[3], pt[5], pt[7] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[3], pt[2], pt[7], pt[6] );

      GameX.DrawTexturedPolygon3D( &redImg, pt[2], pt[0], pt[6], pt[4] );

}

 

void Body::RotateAroundPoint(Vector3DF& position, Vector3DF vCenter, float angle, Vector3DF vAxis)

{

      Vector3DF vNewPosition;

 

      // Get the Vector from our position to the center we are rotating around

      Vector3DF vPos = position - vCenter;

 

      // Calculate the sine and cosine of the angle once

      float cosTheta = (float)cos(angle);

      float sinTheta = (float)sin(angle);

 

      // Find the new x position for the new rotated point

      vNewPosition.x  = (cosTheta + (1 - cosTheta) * vAxis.x * vAxis.x)       * vPos.x;

      vNewPosition.x += ((1 - cosTheta) * vAxis.x * vAxis.y - vAxis.z * sinTheta)   * vPos.y;

      vNewPosition.x += ((1 - cosTheta) * vAxis.x * vAxis.z + vAxis.y * sinTheta)   * vPos.z;

 

      // Find the new y position for the new rotated point

      vNewPosition.y  = ((1 - cosTheta) * vAxis.x * vAxis.y + vAxis.z * sinTheta)   * vPos.x;

      vNewPosition.y += (cosTheta + (1 - cosTheta) * vAxis.y * vAxis.y)       * vPos.y;

      vNewPosition.y += ((1 - cosTheta) * vAxis.y * vAxis.z - vAxis.x * sinTheta)   * vPos.z;

 

      // Find the new z position for the new rotated point

      vNewPosition.z  = ((1 - cosTheta) * vAxis.x * vAxis.z - vAxis.y * sinTheta)   * vPos.x;

      vNewPosition.z += ((1 - cosTheta) * vAxis.y * vAxis.z + vAxis.x * sinTheta)   * vPos.y;

      vNewPosition.z += (cosTheta + (1 - cosTheta) * vAxis.z * vAxis.z)       * vPos.z;

 

      position = vCenter + vNewPosition;

}

 

void Body::RotateAroundAxisX(Vector3DF &newPosition, Vector3DF vPoint, Vector3DF vCenter, float angle)

{

      Vector3DF vNewPosition;

 

      // Get the Vector from our position to the center we are rotating around

      Vector3DF vPos = vPoint - vCenter;

 

      // Calculate the sine and cosine of the angle once

      float cosTheta = (float)cos(angle);

      float sinTheta = (float)sin(angle);

 

      // Find the new x position for the new rotated point

      vNewPosition.x  = vPos.x;

 

      // Find the new y position for the new rotated point

      vNewPosition.y  = (cosTheta * vPos.y) - (sinTheta * vPos.z);

 

      // Find the new z position for the new rotated point

      vNewPosition.z  = (sinTheta * vPos.y) + (cosTheta * vPos.z);

 

      newPosition = (vCenter + vNewPosition);

}

 

void Body::RotateAroundAxisY(Vector3DF &newPosition, Vector3DF vPoint, Vector3DF vCenter, float angle)

{

      Vector3DF vNewPosition;

 

      // Get the Vector from our position to the center we are rotating around

      Vector3DF vPos = vPoint - vCenter;

 

      // Calculate the sine and cosine of the angle once

      float cosTheta = (float)cos(angle);

      float sinTheta = (float)sin(angle);

 

      // Find the new x position for the new rotated point

      vNewPosition.x  = (cosTheta * vPos.x) + (sinTheta * vPos.z);

 

      // Find the new y position for the new rotated point

      vNewPosition.y  = vPos.y;

 

      // Find the new z position for the new rotated point

      vNewPosition.z  = (cosTheta * vPos.z) - (sinTheta * vPos.x);

 

      newPosition = vCenter + vNewPosition;

}

 

 

 

void Body::RotateElbow(float phi, float theta)

{

      theta *= 1/60.0f;

      phi *= -1.0f/60.0f;

      phi -= PI_2;

 

      Vector3DF rotatePoint( (-UPARM_W1_1-UPARM_W1_2)/2, 0, UPARM_H1-THICK );

 

      for (int i = 0; i < 8; i++)

      {

            RotateAroundAxisY( upArm1_pt[i],    orig_upArm1_pt[i],            rotatePoint, theta);

            RotateAroundAxisY( loArm1_pt[i],    orig_loArm1_pt[i],            rotatePoint, theta);

            RotateAroundAxisY( hand1_pt[i],           orig_hand1_pt[i],       rotatePoint, theta);

      }

     

      for (int i = 0; i < 8; i++)

      {

            RotateAroundAxisX( upArm1_pt[i],    upArm1_pt[i],     rotatePoint, phi );

            RotateAroundAxisX( loArm1_pt[i],    loArm1_pt[i],     rotatePoint, phi );

            RotateAroundAxisX( hand1_pt[i],           hand1_pt[i],      rotatePoint, phi );

      }

 

      elbow_phi = phi;

      elbow_theta = theta;

}

 

void Body::RotateWrist(float phi, float psi)

{

      phi *= -1.0f;

      phi -= PI_2;

 

      float avgX, avgY, avgZ;

      avgX = (loArm1_pt[0].x + loArm1_pt[1].x + loArm1_pt[2].x + loArm1_pt[3].x) / 4;

      avgY = (loArm1_pt[0].y + loArm1_pt[1].y + loArm1_pt[2].y + loArm1_pt[3].y) / 4;

      avgZ = (loArm1_pt[0].z + loArm1_pt[1].z + loArm1_pt[2].z + loArm1_pt[3].z) / 4;

      Vector3DF loArmRotatePoint(avgX, avgY, avgZ);

      Vector3DF handRotatePoint(avgX, avgY, avgZ-(LOARM_H1-LOARM_H2));

      tempPt = handRotatePoint;

 

      Vector3DF rotatePhiAxis(cosf(elbow_theta), sinf(elbow_theta), 0);

      Vector3DF rotatePsiAxis = (upArm1_pt[4] - upArm1_pt[0]).Normalize();

 

      for (int i = 0; i < 8; i++)

      {

            loArm1_pt[i]      =           (loArmRotatePoint-orig_loArm1_avg) + orig_loArm1_pt[i];

            hand1_pt[i]       =           (handRotatePoint-orig_hand1_avg) + orig_hand1_pt[i];

 

            RotateAroundPoint(loArm1_pt[i], loArmRotatePoint, phi, rotatePhiAxis);

            RotateAroundPoint(hand1_pt[i], loArmRotatePoint, phi, rotatePhiAxis);        

      }

 

      for (int i = 0; i < 8; i++)

      {

            RotateAroundPoint(loArm1_pt[i], loArmRotatePoint, psi, rotatePsiAxis);

            RotateAroundPoint(hand1_pt[i], loArmRotatePoint, psi, rotatePsiAxis);

      }

 

 

      wrist_phi = phi;

      wrist_psi = psi;

}

 

void Body::Reset()

{

      RotateElbow(0,0);

      RotateWrist(0,0);

}