void Camera::pushMatrix(const osg::Matrixd &mat) { const Matrixd m = mat * qMatrix.back(); glLoadMatrixd(m.ptr()); CHECK_GL(); qMatrix.push_back(m); }
/*************************************************************** * Function: recordPathEntry() * * Write current scale and xform matrix to file with time stamp * ***************************************************************/ void PathRecordManager::recordPathEntry(const double &scale, const Matrixd &xMat) { if (!mFilePtr) return; fwrite(&mTimer,sizeof(double),1,mFilePtr); double scaled = scale; fwrite(&scaled,sizeof(double),1,mFilePtr); fwrite(xMat.ptr(),16*sizeof(double),1,mFilePtr); /*fprintf(mFilePtr, "%f %f ", mTimer, scale); fprintf(mFilePtr, "%f %f %f %f ", xMat(0, 0), xMat(0, 1), xMat(0, 2), xMat(0, 3)); fprintf(mFilePtr, "%f %f %f %f ", xMat(1, 0), xMat(1, 1), xMat(1, 2), xMat(1, 3)); fprintf(mFilePtr, "%f %f %f %f ", xMat(2, 0), xMat(2, 1), xMat(2, 2), xMat(2, 3)); fprintf(mFilePtr, "%f %f %f %f ", xMat(3, 0), xMat(3, 1), xMat(3, 2), xMat(3, 3)); fprintf(mFilePtr, "\n");*/ }
void AndroidNavigator::preFrame() { Matrixd finalmat; double height = 0.0; double magnitude = 1.0; int position = 0; if(ComController::instance()->isMaster()) { int RECVCONST = 48; double x, y, z; x = y = z = 0.0; double rx, ry, rz; rx = ry = rz = 0.0; double sx, sy, sz; sx = sy = sz = 1.0; int tag = 9; int type = 0; int mode = -1; // Navigation = 8, Node = 9, Command = 7 double angle [3] = {0.0, 0.0, 0.0}; double coord [3] = {0.0, 0.0, 0.0}; char send_data[3]; string str; const char* recv_data; char* split_str = NULL; _mutex.lock(); while(!queue.empty()){ str = queue.front(); queue.pop(); split_str = strtok(const_cast<char*>(str.c_str()), " "); type = split_str[1] - RECVCONST; tag = split_str[2] - RECVCONST; mode = split_str[0] - RECVCONST; send_data[0] = type + RECVCONST; send_data[1] = tag + RECVCONST; send_data[2] = '\0'; int tagNum = atoi(send_data); /** * Changes movement type to the tag: * 0 = Manual, 1 = Drive, 2 = Airplane, 3 = Old Fly */ if(type == 2){ _tagCommand = tag; sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); } /** * Process Commands from the android phone * 0 = Connect to server * 1 = Hide Node * 2 = Flip view * 3 = Show Node * 4 = Find AndroidTransform Nodes (to send to phone) * 5 = Gets back a selected AndroidTransform Node from phone, allows nodes to move. * 6 = Use Head Tracking * 7 = Use Device for Orientation Tracking * 8 = reset view */ else if(type == 3) { if(tag == 0){ cout<<"Socket Connected"<<endl; sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); } else if(tag == 1){ sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); node->setNodeMask(0x0); } else if(tag == 2){ sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); angle[1] = PI/rotscale; } else if(tag == 3){ sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); node->setNodeMask(0xffffff); } else if(tag == 4){ sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); AndroidVisitor* visitor = new AndroidVisitor(); SceneManager::instance()->getObjectsRoot()->accept(*visitor); nodeMap = visitor->getMap(); delete visitor; int num = (int) nodeMap.size(); sendto(sock, &num, 4, 0, (struct sockaddr *)&client_addr, addr_len); map<char*, AndroidTransform*>::iterator iter; for(iter=nodeMap.begin(); iter != nodeMap.end(); iter++){ char* name = iter->first; int size = (int) strlen(name); sendto(sock, &size, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); sendto(sock, name, strlen(name), 0, (struct sockaddr *)&client_addr, addr_len); } } else if(tag == 5){ _tagCommand = 5; split_str = strtok(NULL, " "); node_name = new char[strlen(split_str)]; strcpy(node_name, split_str); bool found = false; map<char*, AndroidTransform*>::iterator iter; for(iter=nodeMap.begin(); iter != nodeMap.end(); iter++){ if(strcmp(node_name, iter->first) == 0){ cout<<node_name<<" found"<<endl; node = iter->second; found = true; } } if(!found) cout<<node_name<<" was not found..."<<endl; } else if(tag == 6){ sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); char *sPtr = strtok(NULL, " "); useHeadTracking = (0 == strcmp(sPtr, "true")); } else if(tag == 7){ sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); char *sPtr = strtok(NULL, " "); useDeviceOrientationTracking = (0 == strcmp(sPtr, "true")); } else if(tag == 8){ sendto(sock, &tagNum, sizeof(int), 0, (struct sockaddr *)&client_addr, addr_len); osg::Matrix m; SceneManager::instance()->setObjectMatrix(m); SceneManager::instance()->setObjectScale(1.0); } } /** * Updates Movement data * 0 = Rotation data * 1 = Translation data * 2 = Zcoord Translation * 3 = Velocity * 4 = Node Movement Data * 5 = Orientation angle */ else if (type == 1) { // Updates Rotation data if (tag == 0){ // First angle split_str = strtok(NULL, " "); angle[0] += atof(split_str); // Second angle split_str = strtok(NULL, " "); angle[1] += atof(split_str); // Third angle split_str = strtok(NULL, " "); angle[2] += atof(split_str); } // Updates touch movement data else if (tag == 1){ // First coord split_str = strtok(NULL, " "); coord[0] += atof(split_str); // Second coord split_str = strtok(NULL, " "); coord[1] += atof(split_str); } // Node Adjustment Data else if (tag == 4){ // Height split_str = strtok(NULL, " "); height += atof(split_str); // Magnitude split_str = strtok(NULL, " "); magnitude += atof(split_str); // Axis split_str = strtok(NULL, " "); position = atoi(split_str); } //Orientation else if (tag == 5) { // Angle split_str = strtok(NULL, " "); orientation = atof(split_str); } // Handles pinching movement (on touch screen) and drive velocity else{ split_str = strtok(NULL, " "); if (tag == 2){ coord[2] += atof(split_str); } else if (tag == 3){ velocity = atof(split_str); if(atof(split_str) == 0){ velocity = 0; } } } } } _mutex.unlock(); //angle[1] rot around x //angle[2] rot around y //angle[0] rot around z switch(_tagCommand) { case 0: // For Manual movement rz += angle[0]; rx += angle[1]; ry += angle[2]; if(angle[0] != 0) old_ry = angle[0]; x -= coord[0]; z += coord[1]; y += coord[2]; break; case 1: // For DRIVE movement rz += angle[2]; ry -= coord[0] * .5; // Fixes orientation y += velocity; z += angle[1]; // For vertical movement break; case 2: // For Airplane movement rx += angle[1]; ry -= angle[2]; y += velocity; // allow velocity to scale break; case 3: // Old fly mode rx += angle[1]; ry -= coord[0] * .5; // Fixes orientation rz += angle[2]; y += velocity; // allow velocity to scale break; case 5: if(node_name != NULL){ adjustNode(height, magnitude, position); } else cout<<"No Node Selected"<<endl; break; } // Scales data by set amount x *= transcale; y *= transcale; z *= transcale; rx *= rotscale; rz *= rotscale; ry *= rotscale; /* * If newMode (which occurs when Drive and New Fly starts), * this takes in a new headMat camera pos. * If not, this takes in the old position, with the exception * of the z axis, which corresponds to moving your head up and down * to eliminate conflict between phone and head tracker movement. */ Matrix world2head = PluginHelper::getHeadMat(); Matrix view, mtrans; if(useDeviceOrientationTracking){ view.makeRotate(orientation,0,0,1); mtrans.makeTranslate(world2head.getTrans()); view = view * mtrans; } else view = world2head; Vec3 campos = view.getTrans(); // Gets translation Vec3 trans = Vec3(x, y, z); //Test if(useHeadTracking){ trans = (trans * view) - campos; }else if(useDeviceOrientationTracking){ trans = (trans * view) - campos; } Matrix tmat; tmat.makeTranslate(trans); Vec3 xa = Vec3(1.0, 0.0, 0.0); Vec3 ya = Vec3(0.0, 1.0, 0.0); Vec3 za = Vec3(0.0, 0.0, 1.0); //Test if(useHeadTracking){ xa = (xa * view) - campos; ya = (ya * view) - campos; za = (za * view) - campos; }else if(useDeviceOrientationTracking){ xa = (xa * view) - campos; ya = (ya * view) - campos; za = (za * view) - campos; } // Gets rotation Matrix rot; rot.makeRotate(rx, xa, ry, ya, rz, za); Matrix ctrans, nctrans; ctrans.makeTranslate(campos); nctrans.makeTranslate(-campos); // Calculates new objectMatrix (will send to Slaves). //finalmat = PluginHelper::getObjectMatrix() * nctrans * rot * tmat * ctrans; finalmat = PluginHelper::getObjectMatrix() * nctrans * rot * tmat * ctrans; ComController::instance()->sendSlaves((char *)finalmat.ptr(), sizeof(double[16])); PluginHelper::setObjectMatrix(finalmat); } else { ComController::instance()->readMaster((char *)finalmat.ptr(), sizeof(double[16])); PluginHelper::setObjectMatrix(finalmat); } }