## ultramouse3d_api.cpp

// Karl Gluck
// David DeTomaso
// Written April 2009
#include <math.h> // sqrt
#include "ultramouse3d_api.h"
#include "strsafe.h"

/*

+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,
0,
NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
NULL );

if (!mouse->hIDComDev) return FALSE;

COMMTIMEOUTS comm_timeouts;
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;

COMSTAT ComStat;

ClearCommError(mouse->hIDComDev, &dwErrorFlags, &ComStat);
if (ComStat.cbInQue >= DATA_PACKET_SIZE) {

unsigned char data_packet[DATA_PACKET_SIZE];

// read the data from the queue
data_packet,
DATA_PACKET_SIZE,
NULL);
PurgeComm(mouse->hIDComDev, PURGE_RXCLEAR);
return;
}

// Get the time delays from the packet
data_packet,

// 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
for (int i = Ultramouse3D::HISTORY_LENGTH-1; i > 0; --i) {

}

// find the median of each delay in the history
unsigned short median_delays[3] = {
};

// Uncomment these lines to show unfiltered data

// 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;
}