ultramouse3d_api.cpp
// Karl Gluck
// David DeTomaso
// Written April 2009
#include <math.h> // sqrt
#include "ultramouse3d_api.h"
#include "strsafe.h"
/*
1/2/3 are the ultrasonic receivers
+x
2 ----------------------- 3
+y |
|
| ** computer screen **
|
|
1
(given this CS, +z is INTO the screen)
speed: 330 meters/second
given a delay to each unit, we have 3 sphere equations:
r1^2 = (x-x1)^2 + (y-y1)^2 + (z-z1)^2
r2^2 = (x-x2)^2 + (y-y2)^2 + (z-z2)^2
r3^2 = (x-x3)^2 + (y-y3)^2 + (z-z3)^2
if we define 2=(0,0), this simplifies to:
(delay1 * K)^2 = x^2 + (y-y1)^2 + z^2
(delay2 * K)^2 = x^2 + y^2 + z^2
(delay3 * K)^2 = (x-x3)^2 + y^2 + z^2
where K is the conversion from delay-time to distance
solving for y:
(delay2 * K)^2 - y^2 + (y-y1)^2 = (delay1 * K)^2
(delay2 * K)^2 - y^2 + y^2 + 2*y*y1 - y1^2 = (delay1 * K)^2
(delay2 * K)^2 + 2*y*y1 + y1^2 = (delay1 * K)^2
(delay2 * K)^2 - (delay1 * K)^2 + y1^2 = - 2*y*y1
[(delay2 * K)^2 - (delay1 * K)^2 + y1^2)] / (-2*y1) = y
solving for x:
(delay2 * K)^2 - x^2 = y^2 + z^2
(delay3 * K)^2 = (x-x3)^2 + (delay2 * K)^2 - x^2
(delay3 * K)^2 - (delay2 * K)^2 = (x-x3)^2 - x^2
(delay3 * K)^2 - (delay2 * K)^2 = (x-x3)^2 - x^2
(delay3 * K)^2 - (delay2 * K)^2 = x^2 - 2*x*x3 + x3^2 - x^2
(delay3 * K)^2 - (delay2 * K)^2 = - 2*x*x3 + x3^2
(delay3 * K)^2 - (delay2 * K)^2 - x3^2 = - 2*x*x3
[(delay3 * K)^2 - (delay2 * K)^2 - x3^2] / -2*x3 = x
solving for z, we can use z = sqrt(abs(r2^2 - (x)^2 - (y)^2)) or
(delay2 * K)^2 - (delay1 * K)^2 - (delay3 * K)^2 = -(y-y1)^2 - (x-x3)^2 - z^2
(delay2 * K)^2 - (delay1 * K)^2 - (delay3 * K)^2 + (y-y1)^2 + (x-x3)^2 = z^2
But as found by experimentation, neither of these works particularly well
in practice even though the math is fine.
*/
static const unsigned int UM3DAPI_SERIAL_BAUD = 9600;
static const double UM3DAPI_X3_DIST_METERS = 0.15; // x-coordinate of (3)
static const double UM3DAPI_Y1_DIST_METERS = 0.10; // y-coordinate of (1)
static const double UM3DAPI_SPEED_OF_WAVE_METERS_PER_SEC = 330.0; // speed of sound in air (average)
static const double UM3DAPI_COUNTER_INCREMENT_TO_SECONDS = 1.0 / 160000.0; // frequency of the timer0 interrupt
static const double UM3DAPI_EXP_WEIGHT = 0.05;
int UM3DAPI_MedianCompare(const void* ptr1, const void* ptr2) {
unsigned short v1 = *(const unsigned short*)ptr1;
unsigned short v2 = *(const unsigned short*)ptr2;
int iv1 = v1, iv2 = v2;
return iv1 - iv2;
}
// Find the median of an arbirary list of unsigned shorts by sorting the
// list and returning the one in the center.
unsigned short UM3DAPI_Median(unsigned short* history) {
qsort(history,
Ultramouse3D::HISTORY_LENGTH,
sizeof(unsigned short),
UM3DAPI_MedianCompare);
return history[Ultramouse3D::HISTORY_LENGTH/2];
}
// A drop-in replacement for the median filter; not as nice though
//unsigned short UM3DAPI_Mode(unsigned short* history) {
// qsort(history,
// Ultramouse3D::HISTORY_LENGTH,
// sizeof(unsigned short),
// UM3DAPI_MedianCompare);
// unsigned short number = history[0];
// int times = 1;
// int highest_times = 0;
// unsigned short highest_frequency_number = number;;
// for (int i = 1; i < Ultramouse3D::HISTORY_LENGTH; ++i) {
// if (times > highest_times) {
// highest_times = times;
// highest_frequency_number = number;
// }
// if (number == history[i]) {
// ++times;
// } else {
// times = 0;
// number = history[i];
// }
// }
// return highest_frequency_number;
//}
BOOL UM3DAPI_Create(UINT serial_port, Ultramouse3D* mouse) {
TCHAR port_string[15];
DCB dcb;
ZeroMemory(mouse, sizeof(Ultramouse3D));
wsprintf(port_string, TEXT("COM%d"), serial_port);
mouse->hIDComDev = CreateFile(port_string,
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL );
if (!mouse->hIDComDev) return FALSE;
COMMTIMEOUTS comm_timeouts;
comm_timeouts.ReadIntervalTimeout = 0xFFFFFFFF;
comm_timeouts.ReadTotalTimeoutMultiplier = 0;
comm_timeouts.ReadTotalTimeoutConstant = 0;
comm_timeouts.WriteTotalTimeoutMultiplier = 0;
comm_timeouts.WriteTotalTimeoutConstant = 5000;
SetCommTimeouts(mouse->hIDComDev, &comm_timeouts);
dcb.DCBlength = sizeof(DCB);
GetCommState(mouse->hIDComDev, &dcb);
dcb.BaudRate = UM3DAPI_SERIAL_BAUD;
dcb.ByteSize = 8;
if( !SetCommState(mouse->hIDComDev, &dcb ) ||
!SetupComm(mouse->hIDComDev, 10000, 10000 )){
DWORD dwError = GetLastError();
CloseHandle(mouse->hIDComDev);
return FALSE;
}
return TRUE;
}
VOID UM3DAPI_Destroy(Ultramouse3D* mouse) {
if (!mouse || !mouse->hIDComDev) return;
CloseHandle(mouse->hIDComDev);
ZeroMemory(mouse, sizeof(Ultramouse3D));
}
VOID UM3DAPI_Update(Ultramouse3D* mouse) {
static const unsigned int DATA_PACKET_SIZE = 6;
BOOL bReadStatus;
DWORD dwBytesRead, dwErrorFlags;
COMSTAT ComStat;
// Check to see if we received a full packet; if not, skip to processing
ClearCommError(mouse->hIDComDev, &dwErrorFlags, &ComStat);
if (ComStat.cbInQue >= DATA_PACKET_SIZE) {
unsigned char data_packet[DATA_PACKET_SIZE];
// read the data from the queue
bReadStatus = ReadFile(mouse->hIDComDev,
data_packet,
DATA_PACKET_SIZE,
&dwBytesRead,
NULL);
DWORD dwTotalBytesRead = dwBytesRead;
if (!bReadStatus || dwTotalBytesRead < DATA_PACKET_SIZE) {
PurgeComm(mouse->hIDComDev, PURGE_RXCLEAR);
return;
}
// Get the time delays from the packet
unsigned short receiver_delays[3];
memcpy(receiver_delays,
data_packet,
sizeof(receiver_delays));
// Grab button states. Right now there is only 1; low is pressed.
mouse->button = (receiver_delays[0] & (1<<15)) == 0;
receiver_delays[0] &= ~(1<<15); // clear bit 15
// Add to the delay history
unsigned short receiver_delay_history[3][Ultramouse3D::HISTORY_LENGTH];
for (int i = Ultramouse3D::HISTORY_LENGTH-1; i > 0; --i) {
mouse->receiver_delay_history[0][i] = mouse->receiver_delay_history[0][i - 1];
mouse->receiver_delay_history[1][i] = mouse->receiver_delay_history[1][i - 1];
mouse->receiver_delay_history[2][i] = mouse->receiver_delay_history[2][i - 1];
receiver_delay_history[0][i] = mouse->receiver_delay_history[0][i];
receiver_delay_history[1][i] = mouse->receiver_delay_history[1][i];
receiver_delay_history[2][i] = mouse->receiver_delay_history[2][i];
}
mouse->receiver_delay_history[0][0] = receiver_delays[0];
mouse->receiver_delay_history[1][0] = receiver_delays[1];
mouse->receiver_delay_history[2][0] = receiver_delays[2];
receiver_delay_history[0][0] = mouse->receiver_delay_history[0][0];
receiver_delay_history[1][0] = mouse->receiver_delay_history[1][0];
receiver_delay_history[2][0] = mouse->receiver_delay_history[2][0];
// find the median of each delay in the history
unsigned short median_delays[3] = {
UM3DAPI_Median(receiver_delay_history[0]),
UM3DAPI_Median(receiver_delay_history[1]),
UM3DAPI_Median(receiver_delay_history[2]),
};
// Uncomment these lines to show unfiltered data
//median_delays[0] = receiver_delays[0];
//median_delays[1] = receiver_delays[1];
//median_delays[2] = receiver_delays[2];
// turn the data, which represents delay in counter-increments
// between tx and rx for each of the 3 ultrasonic receivers,
// into time-delays, then into distances
double distance1 = median_delays[0] *
UM3DAPI_COUNTER_INCREMENT_TO_SECONDS *
UM3DAPI_SPEED_OF_WAVE_METERS_PER_SEC;
double distance2 = median_delays[1] *
UM3DAPI_COUNTER_INCREMENT_TO_SECONDS *
UM3DAPI_SPEED_OF_WAVE_METERS_PER_SEC;
double distance3 = median_delays[2] *
UM3DAPI_COUNTER_INCREMENT_TO_SECONDS *
UM3DAPI_SPEED_OF_WAVE_METERS_PER_SEC;
const double distance1sq = distance1*distance1;
const double distance2sq = distance2*distance2;
const double distance3sq = distance3*distance3;
const double y1 = UM3DAPI_Y1_DIST_METERS;
const double x3 = UM3DAPI_X3_DIST_METERS;
const double y1sq = UM3DAPI_Y1_DIST_METERS * UM3DAPI_Y1_DIST_METERS;
const double x3sq = UM3DAPI_X3_DIST_METERS * UM3DAPI_X3_DIST_METERS;
// perform triangulation to find the new mouse coordinate
//[(delay2 * K)^2 - (delay1 * K)^2 + y1^2)] / (-2*y1) = y
double y = (distance2sq - distance1sq + y1sq) / (2*y1) / y1;
// [(delay3 * K)^2 - (delay2 * K)^2 - x3^2] / -2*x3 = x
double x = (distance2sq - distance3sq + x3sq) / (2*x3) / x3;
// First are two mathematically correct but utterly useless
// ways of finding the z-coordinate. The third way is a hack
// (just adding u pthe delays) but it works great.
//(delay1 * K)^2 - (delay2 * K)^2 - (delay3 * K)^2 + (y-y1)^2 + (x-x3)^2 = z^2
//double z = sqrt(abs(distance2sq - y*y - x*x));
//double z = sqrt(abs(distance2sq - distance1sq - distance3sq + (y-y1)*(y-y1) + (x-x3)*(x-x3)));
double z = distance1sq + distance2sq + distance3sq;
// Uncomment these lines to display delay times in the debug output
//TCHAR output[256];
//StringCchPrintf(output, 256,
// TEXT("%i, %i, %i; rx = (%.02f, %.02f, %.02f)\n"),
// median_delays[0],
// median_delays[1],
// median_delays[2],
// float(x),
// float(y),
// float(z));
//OutputDebugString(output);
mouse->target_position.x = x;
mouse->target_position.y = y;
mouse->target_position.z = z;
}
// Update the mouse by smoothing toward the target position by
// an exponentially weighted moving average. This isn't the
// best solution since the speed of approach is based on the
// speed at which this function is called, but it's fine for
// a prototype.
mouse->position.x = (mouse->target_position.x - mouse->position.x) * UM3DAPI_EXP_WEIGHT +
mouse->position.x;
mouse->position.y = (mouse->target_position.y - mouse->position.y) * UM3DAPI_EXP_WEIGHT +
mouse->position.y;
mouse->position.z = (mouse->target_position.z - mouse->position.z) * UM3DAPI_EXP_WEIGHT +
mouse->position.z;
// Uncomment these lines to use unsmoothed mouse data
//mouse->position.x = mouse->target_position.x;
//mouse->position.y = mouse->target_position.y;
//mouse->position.z = mouse->target_position.z;
}
More Information
Implementation file for the UltraMouse 3D API, which lets users easily add the mouse to any Win32/C++ application