Skip to main content



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