/cygdrive/d/src/svn/vrut/trunk/modules/tracking/backup/bodies.cpp

Go to the documentation of this file.

/*
 * $Id: bodies.cpp $
 *
 * Description  : Bodies data structure implementation
 * Author       : Radek Bien <mail: radek.bien@seznam.cz>  <ICQ: 201-963-660>
 *
 * Purpose      :
 *     Implementation of bodies hierarchy
 */
#include "bodies.h"


Body::Body(int id, float qu, Body::BodyType type, float *shift, float *rotation)
{
       deviceID = id;
       quality = qu;
       deviceType = type;
       //this->Identity();                // init matrix with identity
       this->SetRotation(rotation);// copy rotation matrix to transform matrix
       this->SetShift(shift);             // copy shift vector to transform matrix
}
Body::~Body()
{}
void Body::Scale(float num)
{
       vector3 vct;
       vct.x = vct.y =vct.z = num;
       tMatrix.Scale(vct);
}
void Body::SetMatrix(MATRIX & matr)
{
       tMatrix = matr;
}
void Body::Identity()
{
       MATRIX identity;
       tMatrix = identity;
}
void Body::SetShift(float *shift)
{
       if (shift != NULL)
       {
              vector3 vct;
              vct.x = shift[0];
              vct.y = shift[1];
              vct.z = shift[2];
              tMatrix.Translate(vct);
       }
}
void Body::SetRotation(float *rotation)
{
       if (rotation != NULL)
       {
              tMatrix._m11 = rotation[0]; tMatrix._m12 = rotation[3]; tMatrix._m13 = rotation[6];
              tMatrix._m21 = rotation[1]; tMatrix._m22 = rotation[4]; tMatrix._m23 = rotation[7];
              tMatrix._m31 = rotation[2]; tMatrix._m32 = rotation[5]; tMatrix._m33 = rotation[8];
       }
}
int Body::GetID()
{
       return deviceID;
}
Body::BodyType Body::GetType()
{
       return deviceType;
}
vector3 Body::GetShift()
{
       return tMatrix.ExtractTranslation();
}
MATRIX Body::GetRotation()
{
       return tMatrix.ExtractRotation();
}
MATRIX Body::Matrix()
{
       return tMatrix;
}
float Body::GetQuality()
{
       return quality;
}
int Body::Button(int num)
{
       return -10; // standard bodies doesn't have any button
}
int Body::ButonsNumber()
{
       return 0;     // standard bodies doesn't have any button
}
int Body::ControlsNuber()
{
       return 0; // standard bodies doesn't have any button
}
float Body::Control(int num)
{
       return -10.0; // stnadard bodies doesn't have any control
}
int Body::LeftHand()
{
       return -10;     // standard bodies doesn't
}
int Body::FingersNum()
{
       return 0;
}
Finger * Body::GetFinger(int num)
{
       return NULL;
}
BodyGlove::BodyGlove(int id, float qu, int numFingers, bool lHand, float *shift, float *rotation)
: Body(id, qu, Body::BGL, shift, rotation)
{
       this->leftHand = lHand;
       this->fingerCount = numFingers;
       this->fingers = new Finger[fingerCount];
}
BodyGlove::~BodyGlove()
{
       delete [] fingers;
}
void BodyGlove::addFinger(int pos, Finger finger)
{
       if ((0 <= pos) && (pos < fingerCount)) fingers[pos] = finger; // Add finger only if pos is in scope of the array
}
int BodyGlove::LeftHand()
{
       return (this->leftHand ? 1 : 0);
}
int BodyGlove::FingersNum()
{
       return fingerCount;
}
Finger * BodyGlove::GetFinger(int num)
{
       if ((0 <= num) && (num < fingerCount)) return &(fingers[num]);
       else return NULL;
}
ButtonDevice::ButtonDevice(Body::BodyType type, int id, float qu, int buttons, float *shift, float *rotation)
: Body(id, qu, type, shift, rotation)
{
       this->buttons = buttons;
}
ButtonDevice::~ButtonDevice()
{}
int ButtonDevice::ButonsNumber()
{
       return 8;
}
int ButtonDevice::Button(int num)
{
       int res = 0x01;
       if (0<=num && num < 8)
       {
              res = res << num; // shift mask num positions to left
              if (0 != (res & buttons)) return 1;
              else return 0;
       }
       else return -10;
}
FlyStick1::FlyStick1(int id, float qu, int buttons, float *shift, float * rotation)
: ButtonDevice(Body::B6DF,id,qu,buttons,shift,rotation)
{}
FlyStick1::~FlyStick1()
{}
Measurer::Measurer(int id, float qu, int buttons, float *shift, float * rotation)
:ButtonDevice(Body::B6DMT,id,qu,buttons,shift,rotation)
{}
Measurer::~Measurer()
{}
FlyStick2::FlyStick2(int id, float qu, int buttons, int controls, float * shift, float * rotation)
: Body(id, qu, Body::B6DF2, shift, rotation)
{
       buttonsCount = buttons;
       int arrsize = buttons / 32 + 1; // max 32 buttons in 1 configuration
       controlCount = controls;
       this->buttArray = new int[arrsize];
       this->ctrlArray = new float[controls];
}
FlyStick2::~FlyStick2()
{
       delete [] buttArray;
       delete [] ctrlArray;
}
void FlyStick2::addButtons(int pos, int config)
{
       if ((0 <= pos) && (pos < buttonsCount ))
       {
              buttArray[pos] = config;
       }
}
void FlyStick2::addControls(int pos, float value)
{
       if ((0 <= pos) && (pos < controlCount ))
       {
              ctrlArray[pos] = value;
       }
}
int FlyStick2::ButonsNumber()
{
       return buttonsCount;
}
int FlyStick2::ControlsNuber()
{
       return controlCount;
}
float FlyStick2::Control(int num)
{
       if ((0 <= num) && (num < controlCount))   return ctrlArray[num];
       else return -10.0;
}
int FlyStick2::Button(int num)
{
       if ((0 <= num) && (num < buttonsCount))
       {
              int index = num / 32; // number of index to array
              int shift = num % 32; // shift of mask
              int mask = 0x01;
              mask = mask << shift;
              if ( 0 != (mask & buttArray[index])) return 1;
              else return 0;
       }
       else return - 10;
}
int Body::ButtonsMask32()
{
       return 0x00;
}
int ButtonDevice::ButtonsMask32()
{
       return this->buttons;
}
int FlyStick2::ButtonsMask32()
{
       return this->buttArray[0];
}

---