📄 hmdmouse.cpp
字号:
case POS:
posorientcmd[0] = 'V';
break;
case ANGLE:
posorientcmd[0] = 'W';
break;
case MATRIX:
posorientcmd[0] = 'X';
break;
case QUATER:
posorientcmd[0] = 92;
break;
case POSANGLE:
posorientcmd[0] = 'Y';
break;
case POSMATRIX:
posorientcmd[0] = 'Z';
break;
case POSQUATER:
posorientcmd[0] = ']';
break;
default:
return(0);
}
rs232tofbbcmd[0] = (char)(0xF0 | birdaddr); //Set Current Bird's Address
result = WriteFile(m_hCom,rs232tofbbcmd,1 ,&count,&m_ov);
if(!result)
{
switch(dwError = GetLastError())
{
case ERROR_IO_PENDING:
result = GetOverlappedResult(m_hCom, // Handle to COMM port
&m_ov, // Overlapped structure
&count, // Stores number of bytes sent
TRUE); // Wait flag
if(!result)
return 0;
break;
default:
return 0;
break;
}
}
else
{
if(count == 1)
;
else
{
//printf("Failed to set hemisphere...\n");
return 0;
}
}
/*m_ov.Offset = 0;
m_ov.OffsetHigh = 0;
m_ov.hEvent = NULL;*/ //Set output data mode
result = WriteFile(m_hCom,posorientcmd,1,&count,&m_ov);
if(!result)
{
switch(dwError = GetLastError())
{
case ERROR_IO_PENDING:
result = GetOverlappedResult(m_hCom, // Handle to COMM port
&m_ov, // Overlapped structure
&count, // Stores number of bytes sent
TRUE); // Wait flag
if(!result)
return 0;
break;
default:
return 0;
break;
}
}
else
{
if(count == 1)
return(DataMode);
else
{
//printf("Failed to set hemisphere...\n");
return 0;
}
}
//Sleep(DELAYTIME);
}
//////////////////////////////////////////////////////////
// GetSingleBirdData: Get a bird's data //
// DataMode: specify data mode //
// POS,ANGLE,MATRIX,QUATER,POSANGLE, //
// POSMATRIX,POSQUATER //
// MouseMode: 0------cancel 1-----enable //
// *birdPos: bird's Position output buffer //
// *birdOrient:bird's Orientation output buffer//
// *button: mouse button //
// birdaddr: bird's address //
//////////////////////////////////////////////////////////
short CHmdMouse::GetSingleBirdData(short DataMode ,short MouseMode,float *birdPos,
float *birdOrient,short *button,short birdaddr)
{
unsigned char birdcmd[2] = "B";
short datasize;
DWORD count, dwError;
BOOL result;
short birddata[16],*data;
int i;
float floatdata[16];
if (MouseMode) if (!SetMouseMode( birdaddr,MouseMode)) return 0;
if (!SetDataMode(DataMode,birdaddr)) return 0;
/*
set output data's size
*/
switch (DataMode)
{
case POS:
case ANGLE:
datasize = 3;
break;
case MATRIX:
datasize = 9;
break;
case POSANGLE:
datasize = 6;
break;
case POSMATRIX:
datasize = 12;
break;
case QUATER:
datasize = 4;
break;
case POSQUATER:
datasize = 7;
break;
default:
return 0;
}
/*m_ov.Offset = 0;
m_ov.OffsetHigh = 0;
m_ov.hEvent = NULL;*/
result = WriteFile(m_hCom,birdcmd,1,&count,&m_ov);
if(!result)
{
switch(dwError = GetLastError())
{
case ERROR_IO_PENDING:
result = GetOverlappedResult(m_hCom, // Handle to COMM port
&m_ov, // Overlapped structure
&count, // Stores number of bytes sent
TRUE); // Wait flag
if(!result)
return 0;
break;
default:
return 0;
break;
}
}
else
{
if(count == 1)
;
else
{
//printf("Failed to set hemisphere...\n");
return 0;
}
}
// Sleep(DELAYTIME);
data = birddata;
result = ReadFile(m_hCom,(char *)birddata,datasize*2+MouseMode,&count,&m_ov);
if(!result)
{
switch(dwError = GetLastError())
{
case ERROR_IO_PENDING:
result = GetOverlappedResult(m_hCom, // Handle to COMM port
&m_ov, // Overlapped structure
&count, // Stores number of bytes sent
TRUE); // Wait flag
if(!result)
return 0;
break;
default:
return 0;
break;
}
}
else
{
if(count == datasize*2+MouseMode)
;
else
{
//printf("Failed to set hemisphere...\n");
return 0;
}
}
*button = (unsigned)((birddata[datasize] + 512)%256);
*button = (*button + 256) % 256;
/*
MOTOROLA and some RISC CPUs place
the high order byte of a 16 bit word in the lower address of
memory whereas INTEL like CPUs place the high order bytes in the
higher addresses...therefore this operation must be CPU
independent
*/
for (i=0;i<datasize;i++){
*data++ = (short)((((short)(*(unsigned char *) data) & 0x7F) |
(short)(*((unsigned char *) data+1)) << 7)) << 2;
}
data -= datasize;
/*
data change
*/
switch (DataMode)
{
case POS:
for (i=0;i<3;i++) floatdata[i] = (float)(*data++ * POSK36);
break;
case ANGLE:
for (i=0;i<3;i++) floatdata[i] = (float)(*data++ * ANGK);
break;
case MATRIX:
for (i=0;i<9;i++) floatdata[i] = (float)(*data++ * WTF);
break;
case QUATER:
for (i=0;i<4;i++) floatdata[i] = (float)(*data++ * WTF);
break;
case POSANGLE:
for (i=0;i<3;i++) floatdata[i] = (float)(*data++ * POSK36);
for (i=3;i<6;i++) floatdata[i] = (float)(*data++ * ANGK);
break;
case POSMATRIX:
for (i=0;i<3;i++) floatdata[i] = (float)(*data++ * POSK36);
for (i=3;i<12;i++) floatdata[i] = (float)(*data++ * WTF);
break;
case POSQUATER:
for (i=0;i<3;i++) floatdata[i] = (float)(*data++ * POSK36);
for (i=3;i<7;i++) floatdata[i] = (float)(*data++ * WTF);
break;
}
switch(DataMode){
case POS:
case POSANGLE:
case POSMATRIX:
case POSQUATER:
for (i=0;i<3;i++) birdPos[i] = floatdata[i];
for (i=3;i<datasize;i++) birdOrient[i-3] = floatdata[i];
break;
default:
for (i=0;i<datasize;i++) birdOrient[i] = floatdata[i];
}
return datasize;
}
//////////////////////////////////////////////////////////////////////
// BirdAngleAlign - Align the Bird User Specified Direction //
// angle: Specified Direction //
// addr: bird's address //
//////////////////////////////////////////////////////////////////////
void CHmdMouse:: BirdAngleAlign(float *angle,short addr)
{
short i; /* matrix pointer */
char *cdataptr; /* pointer to cdata */
float tempfloat; /* temporary float value */
char cdata[] = {'J',0,0,0,0,0,0,0,0,0,0,0,0}; /* cmd + 12 bytes */
char cmd[2];
/*
convert sines and cosines to configuration bytes in cdata[],
constructing the command string as we go.
*/
cdataptr = &cdata[1]; /* assign pointer to the first character
after the command character
*/
for (i = 0; i < 3; i++)
{
/*
calculate the sine of the angle and
*/
tempfloat = (float)(sin((double)(angle[i] * DTR)));
/*
convert to a word and store in cdata
NOTE: trap for sin(90)...since the bird
can only accept data from -1.000 to 0.99996 (0x8000 to 0x7fff)
*/
if (tempfloat < 0.99998)
{
*cdataptr++ = (unsigned char) ((short) (tempfloat * FTW) & 0x0ff);
*cdataptr++ = (unsigned char) (((short) (tempfloat * FTW) & 0x0ff00) >> 8);
}
else
{
*cdataptr++ = (char)0x0ff;
*cdataptr++ = (char)0x07f;
}
/*
calculate the cosine of the angle and
*/
tempfloat = (float)cos((double)(angle[i] * DTR));
/*
convert to a word and store in cdata
NOTE: trap for cos(0)...since the bird
can only accept data from -1.000 to 0.99996 (0x8000 to 0x7fff)
*/
if (tempfloat < 0.99998)
{
*cdataptr++ = (unsigned char) ((short) (tempfloat * FTW) & 0x0ff);
*cdataptr++ = (unsigned char) (((short) (tempfloat * FTW) & 0x0ff00) >> 8);
}
else
{
*cdataptr++ =(unsigned char) 0x0ff;//
*cdataptr++ = 0x07f;
}
}
/*
Send the Command
*/
// Sleep(DELAYTIME + 10);
unsigned char rs232addr;
DWORD count;
rs232addr = (unsigned char)addr;
cmd[0] = (unsigned char)(0xF0 | rs232addr);
WriteFile(m_hCom,cmd,1,&count,NULL);
Sleep(DELAYTIME );
WriteFile(m_hCom,cdata,13,&count,NULL);
Sleep(DELAYTIME);
}
//////////////////////////////////////////////////////////
// GetDoubleBirdData: Get two birds' data //
// DataMode: specify data mode //
// POS,ANGLE,MATRIX,QUATER,POSANGLE, //
// POSMATRIX,POSQUATER //
// *mousePos: mouse's Position output buffer //
// *mouseOrient:mouse's Orientation output buffer//
// *hmdPos: hmd's Position output buffer //
// *hmdOrient:hmd's Orientation output buffer //
// *button: mouse button //
//////////////////////////////////////////////////////////
short CHmdMouse:: GetDoubleBirdData(short DataMode,
float *mousePos,float *mouseOrient,
float *hmdPos,float *hmdOrient,
short *button)
{
char posorientcmd[10],pos[10] = "B";
char birdchangevaluecmd[] = {'P',0,0,0,0};
BOOL f=0;
short dat[14],flocksize = 2;
short rs232tofbbaddr;
unsigned char rs232tofbbcmd[10];
char cdata[] = {'J',0,0,0,0,0,0,0,0,0,0,0,0};
DWORD count;
char button_cdata[] = {'M',0};
short buttonmode=1,datasize;
float hmddata[16],mousedata[16];
switch(DataMode)
{
case POS:
posorientcmd[0] = 'V';
datasize =3;
break;
case ANGLE:
posorientcmd[0] = 'W';
datasize = 3;
break;
case MATRIX:
posorientcmd[0] = 'X';
datasize = 9;
break;
case QUATER:
posorientcmd[0] = 92;
datasize = 4;
case POSANGLE:
posorientcmd[0] = 'Y';
datasize=6;
break;
case POSMATRIX:
posorientcmd[0] = 'Z';
datasize = 12;
break;
case POSQUATER:
posorientcmd[0] = ']';
datasize = 7;
break;
default:return 0;
}
//cgj
rs232tofbbaddr = (unsigned char)m_MouseAddr;
rs232tofbbcmd[0] = (unsigned char)( 0xF0 | rs232tofbbaddr); //Set Current Bird's Address
WriteFile(m_hCom,rs232tofbbcmd,1 ,&count,NULL);
Sleep(DELAYTIME);
button_cdata[1] = (char)buttonmode; //Set 3Dmouse Button mode
WriteFile(m_hCom,button_cdata,2,&count,NULL);
Sleep(DELAYTIME);
//Set Euler Output Mode
WriteFile(m_hCom,posorientcmd,1,&count,NULL);
Sleep(DELAYTIME);
rs232tofbbaddr = (unsigned char)m_HmdAddr;
rs232tofbbcmd[0] = (unsigned char)(0xF0|rs232tofbbaddr); //Set Current Bird's Address
WriteFile(m_hCom,rs232tofbbcmd,1,&count,NULL);
Sleep(DELAYTIME);
//Set Euler Output Mode
WriteFile(m_hCom,posorientcmd,1,&count,NULL);
Sleep(DELAYTIME);
//Set mouse address
rs232tofbbaddr = (unsigned char)m_MouseAddr;
rs232tofbbcmd[0] = (unsigned char)(0xF0|rs232tofbbaddr);
WriteFile(m_hCom,rs232tofbbcmd,1,&count,NULL);
Sleep(DELAYTIME);
// set dot output mode
WriteFile(m_hCom,pos,1,&count,NULL);
Sleep(DELAYTIME);
//read data from mouse
ReadFile(m_hCom,(char *)dat,2*datasize+1,&count,NULL);
Sleep(DELAYTIME);
//read mouse button
*button = (unsigned)((dat[datasize]+512)%256);
*button = (*button+256)%256;
ByteToData(DataMode,dat,mousedata);
//Set HMD address
rs232tofbbaddr = m_HmdAddr;
rs232tofbbcmd[0] = (unsigned char)(0xF0| (unsigned char)rs232tofbbaddr);
WriteFile(m_hCom,rs232tofbbcmd,1,&count,NULL);
Sleep(DELAYTIME);
//SEt dot output mode
WriteFile(m_hCom,pos,1,&count,NULL);
Sleep(DELAYTIME);
//read data from HMD
ReadFile(m_hCom,(char *)dat,2*datasize,&count,NULL);
Sleep(DELAYTIME);
ByteToData(DataMode,dat,hmddata);
short i;
switch(DataMode){
case POS:
case POSANGLE:
case POSMATRIX:
case POSQUATER:
for (i=0;i<3;i++)
{
mousePos[i] = mousedata[i];
hmdPos[i] = hmddata[i];
}
for (i=3;i<datasize;i++)
{
mouseOrient[i-3] = mousedata[i];
hmdOrient[i-3] = hmddata[i];
}
break;
default:
for (i=0;i<datasize;i++)
{
mouseOrient[i] = mousedata[i];
hmdOrient[i] = hmddata[i];
}
}
return datasize;
}
///////////////////////////////////////////////////////
//// GetHmdAddress: get hmd's bird address ///
///////////////////////////////////////////////////////
short CHmdMouse :: GetHmdAddress()
{
return m_HmdAddr;
}
///////////////////////////////////////////////////////////
//// GetHmdAddress: get 3dmouse's bird address ///
///////////////////////////////////////////////////////////
short CHmdMouse :: Get3DmouseAddress()
{
return m_MouseAddr;
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -