void ClipPlaneChunk::deactivate(DrawEnv *, UInt32 idx)
{
#if !defined(OSG_OGL_COREONLY) || defined(OSG_CHECK_COREONLY)
    if(getEnable() == true)
    {
        glDisable( GL_CLIP_PLANE0 + idx);
    }
#else
    OSG_ASSERT(false);
#endif
}
bool ClipPlaneChunk::operator == (const StateChunk &other) const
{
    ClipPlaneChunk const *tother = dynamic_cast<ClipPlaneChunk const*>(&other);

    if(!tother)
        return false;

    if(getEnable() != tother->getEnable())
        return false;

    if(getEquation() != tother->getEquation())
        return false;

    if(getBeacon() != tother->getBeacon())
        return false;
    
    return true;
}
void ClipPlaneChunk::activate(DrawEnv *pEnv, UInt32 idx)
{
#if !defined(OSG_OGL_COREONLY) || defined(OSG_CHECK_COREONLY)
    pEnv->incNumChunkChanges();

    Matrix beaconMat;

    Matrix cameraMat = pEnv->getCameraViewing();

    if(getBeacon() != NULL) 
    {
        getBeacon()->getToWorld(beaconMat);
    }
    else
    {
        beaconMat.setIdentity();

        SWARNING << "NO beacon" << std::endl;
    }

    cameraMat.mult(beaconMat);
    
    if(getEnable() == true)
    {
              GLdouble  glEq[4];
        const Vec4f    &eq = getEquation();

        glEq[0] = eq[0];
        glEq[1] = eq[1];
        glEq[2] = eq[2];
        glEq[3] = eq[3];
        
        glPushMatrix();
        glLoadMatrixf(cameraMat.getValues());
        
        glClipPlane( GL_CLIP_PLANE0 + idx, glEq);
        glEnable( GL_CLIP_PLANE0 + idx);
        
        glPopMatrix();
    }
#else
    OSG_ASSERT(false);
#endif
}
void ClipPlaneChunk::changeFrom(DrawEnv    *pEnv, 
                                StateChunk *old_chunk, 
                                UInt32      idx)
{
#if !defined(OSG_OGL_COREONLY) || defined(OSG_CHECK_COREONLY)
    ClipPlaneChunk const *old = dynamic_cast<ClipPlaneChunk const*>(old_chunk);

    // change from me to me?
    // this assumes I haven't changed in the meantime. 
    // is that a valid assumption?

    if(old == this)
        return;

    pEnv->incNumChunkChanges();

    Matrix beaconMat;

    Matrix cameraMat = pEnv->getCameraViewing();

    if(getBeacon() != NULL) 
    {
        getBeacon()->getToWorld(beaconMat);
    }
    else
    {
        beaconMat.setIdentity();

        SWARNING << "ClipPlaneChunk::changeFrom: NO beacon" << std::endl;
    }
    
    cameraMat.mult(beaconMat);
    
    if(getEquation() != old->getEquation() ||
       getEnable  () != old->getEnable  () ||
       getBeacon  () != old->getBeacon  ()   )
    {
        if(getEnable() == true)
        {
            GLdouble     glEq[4];
            const Vec4f &eq = getEquation();

            glEq[0] = eq[0];
            glEq[1] = eq[1];
            glEq[2] = eq[2];
            glEq[3] = eq[3];
            
            glPushMatrix();
            glLoadMatrixf(cameraMat.getValues());
            
            glClipPlane(GL_CLIP_PLANE0 + idx, glEq);
            glEnable   (GL_CLIP_PLANE0 + idx      );
            
            glPopMatrix();
        }
        else  
        {
            glDisable(GL_CLIP_PLANE0 + idx);
        }
    }
#else
    OSG_ASSERT(false);
#endif
}
Exemple #5
0
int main(int argc, char* argv[]) {
	printf("SimpleDS\n");
	printf("Build: " VERSION_STR " (" SYSTEM_NAME " " SYSTEM_PROCESSOR ")");
#ifdef SDL_HAPTIC_DISABLED
	printf(" - No Haptic");
#endif
	printf("\nAuthors: " VERSION_AUTHORS "\n");

	args = std::vector<const char*>(argv, argv + argc);
	int offset = 1;

	std::string configFile = "./simpleds.conf";
	if (hasOpt("-c") || hasOpt("--config")) {
		configFile = getOpt("-c");
		if (configFile.size() == 0) {
			configFile = getOpt("--config");
		}
		offset += 2;
	}
	config = new Config(configFile);
	if (config->loaded) {
		printf("Config: %s\n", configFile.c_str());
		if (config->has("DS.foo")) {
			printf("Foo: %s\n", config->getString("DS.foo").c_str());
		}
	} else {
		printf("Config file couldn't be loaded, won't be able to save\n");
	}

	uint16_t teamNum = (uint16_t)config->getInt32("DS.team");

	verbose = (hasOpt("-v") || hasOpt("--verbose"));

	if (hasOpt("-V") || hasOpt("--version")) {
		return 0;
	}

	if (hasOpt("-h") || hasOpt("--help")) {
		printUsage();
		printf("Options:\n");
		printf(" -h, --help      Prints this message\n");
		printf(" -v, --verbose   Output debugging information\n");
		printf(" -V, --version   Prints version info and exits\n");
		printf(" -c, --config    Sets the config file to use [default: ./simpleds.conf]\n");
		printf(" teamNum         The team number to use, must be provided here or in configuration file\n");
		return 0;
	}

	if (teamNum == 0 && args.begin() + offset == args.end()) { // We're out of arguments
		printUsage();
		return 1;
	} else {
		char* endptr;
		uint16_t argTeamNum = (uint16_t)std::strtol(args[offset], &endptr, 10);

		if (endptr == args[offset] || *endptr != '\0') { // No team number in config, and none provided
			if (teamNum == 0) {
				printUsage();
				return 1;
			}
		} else {
			teamNum = argTeamNum;
		}
	}

	printf("Team Number: %d\n", teamNum);

	bool quit = false;
	SDL_Event e;

	auto gui = new GUI();
	if (!gui->isValid()) {
		return 1;
	}

	config->setInt32("DS.team", teamNum);
	config->initInt32("DS.alliance", Alliance::RED);
	config->initInt32("DS.position", 1);

	DS::initialize(teamNum);
	auto ds = DS::getInstance();

	enum GUIMode { MAIN, INFO, JOYSTICKS, CONTROL, HELP, COUNT };
	GUIMode mode = GUIMode::MAIN;

	auto runner = std::async(std::launch::async, &DS::run, ds);
	std::map<GUIMode, Screen*> screens;
	screens[MAIN] = new ScreenMain();
	screens[INFO] = new ScreenInfo();
	screens[JOYSTICKS] = new ScreenJoysticks();
	screens[CONTROL] = new ScreenControl();
	screens[HELP] = new ScreenHelp();

	while (!quit) {
		while (gui->pollEvent(&e) != 0) {
			if (e.type == SDL_QUIT) {
				quit = true;
			} else if (e.type == SDL_JOYDEVICEREMOVED || e.type == SDL_JOYDEVICEADDED) {
				ds->setEnable(false);
				ds->loadJoysticks();
			} else if (e.type == SDL_KEYDOWN && e.key.repeat == 0) {
				auto key = e.key.keysym;
				if (key.sym == SDLK_e) {
					ds->toggleEnable();
				} else if (key.sym == SDLK_SPACE) {
					ds->setEnable(false);
				} else if (key.sym == SDLK_0) {
					ds->setEStop();
				} else if (key.sym == SDLK_q) {
					quit = true;
				} else if (key.sym == SDLK_r) {
					if (key.mod & KMOD_SHIFT) {
						ds->reboot();
					} else {
						ds->restartCode();
					}
				} else if (key.sym == SDLK_BACKQUOTE) {
					ds->setAlliance(ds->getAlliance() == Alliance::BLUE ? Alliance::RED : Alliance::BLUE);
				} else if (key.sym >= SDLK_1 && key.sym <= SDLK_9) {
					uint8_t keyNum = (uint8_t)(1 + key.sym - SDLK_1);
					if (key.mod & KMOD_CTRL && keyNum >= 1 && keyNum <= 3) {
						ds->setPosition(keyNum);
					} else if (key.mod & KMOD_SHIFT && keyNum >= 1 && keyNum <= 3) {
						ds->setEnable(false);
						ds->setMode((Mode)(keyNum - 1));
					} else if (keyNum >= 1 && keyNum <= GUIMode::COUNT) {
						mode = (GUIMode)(keyNum - 1);
					}
				}
			}
			screens[mode]->update(e);
		}
		if (gui->readyToDraw()) {
			gui->setOffset(10, 10);
			gui->clear();
			gui->drawScreen(screens[mode]);

			gui->setOffset(10, gui->getHeight() - gui->getCharSize().y);
			gui->drawText(0, 0, "1: Main", mode == GUIMode::MAIN ? Colors::BLACK : Colors::DISABLED);
			gui->drawTextRel(9, 0, "2: Info", mode == GUIMode::INFO ? Colors::BLACK : Colors::DISABLED);
			gui->drawTextRel(9, 0, "3: Joysticks", mode == GUIMode::JOYSTICKS ? Colors::BLACK : Colors::DISABLED);
			gui->drawTextRel(14, 0, "4: Control", mode == GUIMode::CONTROL ? Colors::BLACK : Colors::DISABLED);
			gui->drawTextRel(12, 0, "5: Help", mode == GUIMode::HELP ? Colors::BLACK : Colors::DISABLED);

			gui->render();
		}

		std::string s = narf::util::format("Team %d", teamNum);
		if (ds->isConnected()) {
			auto rio = ds->getRoboRIO();
			s += " - ";
			if (rio->getEStop()) {
				s += "E-Stopped";
			} else {
				s += narf::util::format("%s %s", modeNames[rio->getMode()].c_str(), rio->getEnable() ? "Enabled" : "Disabled");
			}
			s += narf::util::format(" - %s %d", allianceNames[ds->getAlliance()].c_str(), ds->getPosition());
			if (!rio->getCode()) {
				s += " - No Code";
			}
		} else {
			s += " - No Comms";
		}
		gui->setTitle(s);

		ds->updateJoysticks();
		SDL_Delay(25);
	}

	ds->saveJoysticks();
	ds->stop();
	SDL_Quit();
	return 0;
}
void PhysicsGeom::changed(ConstFieldMaskArg whichField, 
                            UInt32            origin,
                            BitVector         details)
{
    Inherited::changed(whichField, origin, details);

    //Do not respond to changes that have a Sync origin
    if(origin & ChangedOrigin::Sync)
    {
        return;
    }

    if(whichField & BodyFieldMask)
    {
        if(getBody() != NULL)
        {
	        dGeomSetBody(_GeomID, getBody()->getBodyID());
        }
        else
        {
	        dGeomSetBody(_GeomID, 0);
        }
    }
    if(whichField & PositionFieldMask)
    {
	    dGeomSetPosition(_GeomID, getPosition().x(),getPosition().y(),getPosition().z());
    }
    if(whichField & RotationFieldMask)
    {
	    dMatrix3 rotation;
	    Vec4f v1 =  getRotation()[0];
	    Vec4f v2 =  getRotation()[1];
	    Vec4f v3 =  getRotation()[2];
	    rotation[0]   = v1.x();
	    rotation[1]   = v1.y();
	    rotation[2]   = v1.z();
	    rotation[3]   = 0;
	    rotation[4]   = v2.x();
	    rotation[5]   = v2.y();
	    rotation[6]   = v2.z();
	    rotation[7]   = 0;
	    rotation[8]   = v3.x();
	    rotation[9]   = v3.y();
	    rotation[10]  = v3.z();
	    rotation[11]  = 0;
	    dGeomSetRotation(_GeomID, rotation);
    }
    if(whichField & QuaternionFieldMask)
    {
	    dQuaternion q;
	    q[0]=getQuaternion().w();
	    q[1]=getQuaternion().x();
	    q[2]=getQuaternion().y();
	    q[3]=getQuaternion().z();
	    dGeomSetQuaternion(_GeomID,q);
    }
    if(whichField & OffsetPositionFieldMask &&
        getBody() != NULL)
    {
	    dGeomSetOffsetPosition(_GeomID, getOffsetPosition().x(),getOffsetPosition().y(),getOffsetPosition().z());
    }
    if(whichField & OffsetRotationFieldMask &&
        getBody() != NULL)
    {
	    dMatrix3 rotation;
	    Vec4f v1 =  getOffsetRotation()[0];
	    Vec4f v2 =  getOffsetRotation()[1];
	    Vec4f v3 =  getOffsetRotation()[2];
	    rotation[0]   = v1.x();
	    rotation[1]   = v1.y();
	    rotation[2]   = v1.z();
	    rotation[3]   = 0;
	    rotation[4]   = v2.x();
	    rotation[5]   = v2.y();
	    rotation[6]   = v2.z();
	    rotation[7]   = 0;
	    rotation[8]   = v3.x();
	    rotation[9]   = v3.y();
	    rotation[10]  = v3.z();
	    rotation[11]  = 0;
	    dGeomSetOffsetRotation(_GeomID, rotation);
    }
    if(whichField & OffsetQuaternionFieldMask && getBody() != NULL)
    {
	    dQuaternion q;
	    q[0]=getOffsetQuaternion().w();
	    q[1]=getOffsetQuaternion().x();
	    q[2]=getOffsetQuaternion().y();
	    q[3]=getOffsetQuaternion().z();
	    dGeomSetOffsetQuaternion(_GeomID,q);
    }
    if(whichField & CategoryBitsFieldMask)
    {
	    dGeomSetCategoryBits(_GeomID, getCategoryBits());
    }
    if(whichField & CollideBitsFieldMask)
    {
	    dGeomSetCollideBits(_GeomID, getCollideBits());
    }
    if(whichField & SpaceFieldMask)
    {
        dSpaceID CurSpace(dGeomGetSpace(_GeomID));
     
        if(CurSpace != 0 &&
           (getSpace() == NULL ||
            CurSpace != getSpace()->getSpaceID()))
        {
            dSpaceRemove(CurSpace,_GeomID);
        }

        if(getSpace() != NULL)
        {
	        dSpaceAdd(getSpace()->getSpaceID(), _GeomID);
        }
    }
    if(whichField & EnableFieldMask)
    {
        if(getEnable())
        {
            dGeomEnable(_GeomID);
        }
        else
        {
            dGeomDisable(_GeomID);
        }
    }
}