/Users/craigcornelius/Projects/SPRING Mac Release 0.2/haptic_v2.cpp

Go to the documentation of this file.

// $Id: haptic_v2.cpp,v 1.9 2001/10/17 16:49:35 kevin Exp $
//
//  Haptic_v2.cpp
//

#include "haptic_v2.h"
#include <stdio.h>
#include <stdlib.h>
#include "space.h"
#include "triangles.h"
#include "void.h"

#ifdef _WIN32
#else
#include <string.h>
#endif

// port number to connect to
#define HSOCK 9999

// buffer size- well under the 1500 bytes ethernet limit, 
// so will fit in 1 packet
#define BUFSIZE 300

const char* Haptic_v2::rcsid = "@(#) $Id: haptic_v2.cpp,v 1.9 2001/10/17 16:49:35 kevin Exp $ $Copyright: (c)2001 National Biocomputation Center, Stanford University $";

int Haptic_v2::debug = 0;

Haptic_v2::Haptic_v2(char* machine_name)
: Sensor(Sensor::haptic_v2_sensor)
{
    num_active = 1;
    
    hash = 0;
    my_space_t = new space_t();
    my_space_v = new space_v();
    my_space_i = new space_i();
    points_to_save = new space_i();
    my_space = my_space_v;
    
    // set up socket:
    buf = (char *)malloc(sizeof(char)*BUFSIZE);
    sizebuf=BUFSIZE;
    char server[80]; // = "maki.stanford.edu";
    strncpy(server, machine_name, 80);
    
    cerr << "making new socket to: " << server << endl;
    hsock = new Socket(server,Socket::tcp,HSOCK);
    cerr << "created socket" << endl;
    
    // send initial handshake
    sprintf(buf, "haptic v2.0");
    hsock->Send(buf,BUFSIZE);
    
    // Check to make sure haptic responded with an acknowledgement
    // (should ideally respond with what type of device it is, so we can
    // do device-specific things on this side (like remap coords, etc))
    hsock->Receive(buf,BUFSIZE);
    cerr << "BUF: [" << buf << "]\n";
    
    // test their response- do they support this version of the protocol?
    if (strncmp(buf,"ACK",3) != 0) {
        // no ACK, so an error has occured and we cannot connect
        cerr << "ERROR - denied connection" << endl;
    } else {
        // success
        cerr << "Connection made" << endl;
    }
    
}

Haptic_v2::~Haptic_v2()
{
    us_t l;
    buf[0] = TYPE_QUIT;
    l.s = htonl(0);
    for(int i=0; i<2; i++)
        buf[1+i] = l.c[i];
    hsock->Send(buf, 3);
    delete hsock;
}

void Haptic_v2::Update()
{
    
    char headers[3];
    char *oldbuf;
    unsigned short size;
    space_type new_type;
    
    //
    // Send the space if it changed.
    //
    
    if(this->spaceChanged())
    {
        //    cerr << "points_to_save: " << points_to_save->my_points->size() << endl;
        //    cerr << "my_space_i: " << my_space_i->my_points->size() << endl;
        //    cerr << "my_space_t: " << my_space_t->my_triangles->size() << endl;
        my_space->send_datas(hsock);
        my_space->print_datas();
    }
#if 0
    if(this->spaceChanged() && (my_space != my_space_t))
    {
        my_space->send_datas(hsock);
    }
#endif
    
    //
    // Get the position (and later, rotation matrix)
    //
    
    //  cout << "RECEIVED" << endl;
    // get the buffer
    
    while(hsock->InputQueueSize() >= 3)
    {
        if(hsock->Receive(headers, 3) == 3)
        {
            new_type = headers[0];
            receive_ushort_from_buf(headers + 1, &size);
            if(size > sizebuf)
            {
                oldbuf = buf;
                buf = (char *) realloc(oldbuf, size * sizeof(char));
                if(buf)
                    sizebuf = size;
                else
                {
                    cerr << "problem of memory... I will go mad," << endl;
                    buf = oldbuf;
                }
            }
            if(size <= sizebuf) {
                if(new_type == SEND_QUIT) {
                    cerr << "The haptics did quit... I will go mad." << endl;
                }
                if(hsock->Receive(buf, size) != size) {
                    cerr << "I didn't receive a packet of the right length..." << endl;
                    cerr << "I will go mad," << endl;
                }
                else if (hsock->InputQueueSize() < 3) {
                    switch(headers[0])
                    {
                    case SEND_QUIT : break;
                    case SEND_COORD:
                        if(size == 56) {
                            int i,j;
                            double position[12];
                            double grip;
                            long touching;
                            char *index = buf;
                            for(i=0; i<12; i++)
                            {
                                receive_float_from_buf(index, position + i);
                                index+=4;
                            }
                            receive_float_from_buf(index, &grip); index+=4;
                            receive_long_from_buf(index, &touching); index+=4;
                            
                            // unpack the position
                            pos.x = position[0];
                            pos.y = position[1];
                            pos.z = position[2];
                            pos -= init_pos;    // center at user-given origin
                            pos *= scalefactor; // and scale it by the given factor
                            
                            // unpack the rotation matrix
                            for(i=0; i<3;i++)
                                for(j=0;j<3;j++)
                                    m.m[i][j] = position[3 + j + 3*i];
                                active[0] = float(grip);
                        }
                        else {
                            cerr << "I received a coords packets of the wrong size." << endl;
                            cerr << "Ignoring it." << endl;
                        } 
                    } // end switch
                }
            }  // size <= sizebuf
        }
        else {
            cerr << "I just received a packet with size < 3..." << endl;
        }
    }
#if 0
    {
        
        hsock->Receive(buf,BUFSIZE);
        
        // parse the position info
        Point3D hpos;
        double hm[3][3];
        double hactive;
        sscanf(buf,"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
            &hpos.x, &hpos.y, &hpos.z,
            &hm[0][0], &hm[0][1], &hm[0][2], &hm[1][0], &hm[1][1], &hm[1][2],
            &hm[2][0], &hm[2][1], &hm[2][2], &hactive);
        
        // and remap it
        pos.x = hpos.y;
        pos.y = hpos.x;
        pos.z = -hpos.z;
        pos -= init_pos;        // center at user-given origin
        pos *= scalefactor;     // and scale it by the given factor
        {
            int i,j;
            for(i=0; i<3; i++)
                for(j=0; j<3; j++)
                    m[i][j] = hm[i][j];
        }
        active = hactive;
        //  cout << "BUF: " << buf << endl;
        //****************************
    }
#endif
}

int Haptic_v2::spaceChanged()
{
    static long hash = 0;
    static long tmp;
    
    tmp = my_space->gethash();
    if(tmp != hash)
    {
        hash = tmp;
        return 1;
    }
    return 0;
}

---