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