void FluidMask::createObstacles(float spacing)
{
cout<<"Parsing svg paths... ";
int obsNum=0;
int pathNum = svg.getNumPath();
for (int i=0; i<pathNum; i++)
{
ofPath path = svg.getPathAt(i);
if (path.getStrokeColor().r != 0) {
continue;
}
// for each path get the outline
vector<ofPolyline> outline = path.getOutline();
for (int l=0; l<outline.size(); l++)
{
ofPolyline resampledLine = outline[l].getResampledBySpacing(spacing);
vector<ofPoint> points = resampledLine.getVertices();
for (int v=0; v<points.size(); v++)
{
// cout<<"vertex at "<<points[v].x<<"x"<<points[v].y<<endl;
addObstacle(points[v].x/2, points[v].y/2 - 5, obstacleSize);
obsNum++;
}
}
}
cout<<"created "<<obsNum<<" obstacles\n";
}
void Game::init() {
objectMap.addLayer(Obstacle::STATIC, AABBf(V2f(-60, -60), V2f(60, 60)), V2i(16, 16));
addObstacle(new CircleObject(Circf(V2f(-4, -4), 2), Obstacle::STATIC));
addObstacle(new AABBObject(AABBf(V2f(-4, 1), V2f(-2, 5)), Obstacle::STATIC));
addObstacle(new BoxObject(Boxf(V2f(4, 0), V2f(2, 5), V2f(1)), Obstacle::STATIC));
addObstacle(new ConeObject(Conef(V2f(0, 1), 1.5f, (float)V_2PI * 1 / 8, (float)V_2PI * 3 / 8), Obstacle::STATIC)); // make the radius of the cone correct based on the calculated center
V2f trapezoid[] = { V2f(-.5f, .5f), V2f(.5f, 1.2f), V2f(.5f, -1.5f), V2f(-.5f, -.8f) };
int trapezoidCount = sizeof(trapezoid) / sizeof(trapezoid[0]);
addObstacle(new PolygonObject(Polygon2f(V2f(5, -7), V2f((float)V_2PI*0.0f/6), trapezoid, trapezoidCount), Obstacle::STATIC)); // TODO raycast does not always hit the shape correctly from all sides
V2f a(-2, -2), b(5, -4);
addObstacle(new LineObject(Linef(&a, &b), Obstacle::STATIC));
addObstacle(new PointObject(V2f(7, 7), Obstacle::STATIC));
delaunyBoundary = new CircleObject(Circf(V2f::ZERO(), 100), Obstacle::EVERYTHING);
delauny = new DelaunySet(delaunyBoundary);
//delauny->addNode(V2f(3, 4));
//delauny->addNode(V2f(-3, 4));
//delauny->addNode(V2f(-3, -4));
//delauny->addNode(V2f(0, -1));
// for (int i = 0; i < 10; ++i) {
// delauny->addNode(V2f::randomUnitVector() * Random::PRNGf() * 100);
// }
delauny->makeRandom(6);
delauny->calculateAllTriangles();
}
void CliHandler::operator()(const int sockfd) {
for (;;) {
std::string line = fetchMessageString(sockfd);
auto cmd = detachToken(line);
makeLowerCase(cmd);
if (cmd == "at")
addTarget(line);
else if (cmd == "ao")
addObstacle(line);
else if (cmd == "clear")
clear(line);
else if (cmd == "eot")
return;
}
}
void updateMap(struct comm_range_pose_data * data) {
int i;
double theta_pose;
// Read pose
struct point_d pose;
pose.x = data->pose[INDEX_X];
pose.y = data->pose[INDEX_Y];
theta_pose = data->pose[INDEX_A];
free(current_pose);
current_pose = gridify(&pose);
send_msg.obs_count = 0;
// Convert ranges absolute positions
for (i = 0; i < RANGE_COUNT; i++) {
struct point_d obstacle_l, obstacle_g;
// obstacle location relative to the robot
double tao = (2.0 * M_PI * i) / RANGE_COUNT;
obstacle_l.x = data->ranges[i] * cos(tao);
obstacle_l.y = data->ranges[i] * sin(tao);
// obstacle location in global coords
obstacle_g.x = obstacle_l.x * cos(theta_pose) - obstacle_l.y * sin(theta_pose);
obstacle_g.x += pose.x;
obstacle_g.y = obstacle_l.x * sin(theta_pose) + obstacle_l.y * cos(theta_pose);
obstacle_g.y += pose.y;
addObstacle(gridify(&obstacle_g));
}
// TODO: What if a SDC messed with the obs_count sent?
if (insertSDC) {
insertSDC = false;
current_pose->x++;
}
send_msg.pose_x = current_pose->x;
send_msg.pose_y = current_pose->y;
commSendMapUpdate(&pipes[update_index], &send_msg);
}
void
World::init()
{
Material material = {
Options::mu(),
Options::lambda(),
Options::density(),
Options::hardening(),
Options::flowRate(),
Options::yieldPoint(),
};
SoftBody* body = new SoftBody(Options::particleFile(), material);
Mesh* mesh = new Mesh;
if (Mesh::loadObj(Options::meshFile(), *mesh)) {
body->setMesh(mesh);
}
addSoftBody(body);
addObstacle(new PlaneObstacle(Eigen::Vector3d(0, 1, 0), -1, Options::friction()));
}
void ModularStateEstimator::init(core::ConfigNode config,
core::EventHubPtr eventHub)
{
//Removed 7/13/2013 to test Combined DVL Acclerometer Kalman filter
//modules.push_back(EstimationModulePtr(
// new BasicDVLEstimationModule(
// config["DVLEstimationModule"],
// eventHub,
// m_estimatedState)));
modules.push_back(EstimationModulePtr(
new DVLAccelerometerEstimator(
eventHub,
m_estimatedState)));
modules.push_back(EstimationModulePtr(
new BasicIMUEstimationModule(
config["IMUEstimationModule"],
eventHub,
m_estimatedState)));
modules.push_back(EstimationModulePtr(
new DepthKalmanModule(
config["DepthEstimationModule"],
eventHub,
m_estimatedState)));
if(config.exists("GreenBuoy"))
{
addObstacle(config["GreenBuoy"], Obstacle::GREEN_BUOY);
modules.push_back(
EstimationModulePtr(new SimpleBuoyEstimationModule(
config["GreenBuoyEstimationModule"],
eventHub,
m_estimatedState,
Obstacle::GREEN_BUOY,
vision::EventType::BUOY_FOUND)));
}
if(config.exists("RedBuoy"))
{
addObstacle(config["RedBuoy"], Obstacle::RED_BUOY);
modules.push_back(
EstimationModulePtr(new SimpleBuoyEstimationModule(
config["RedBuoyEstimationModule"],
eventHub,
m_estimatedState,
Obstacle::RED_BUOY,
vision::EventType::BUOY_FOUND)));
}
if(config.exists("YellowBuoy"))
{
addObstacle(config["YellowBuoy"], Obstacle::YELLOW_BUOY);
modules.push_back(
EstimationModulePtr(new SimpleBuoyEstimationModule(
config["YellowBuoyEstimationModule"],
eventHub,
m_estimatedState,
Obstacle::YELLOW_BUOY,
vision::EventType::BUOY_FOUND)));
}
}
dtStatus dtTileCache::addObstacle(const float* pos, const float radius, const float height, dtObstacleRef* result)
{
return addObstacle(pos, radius, height, 0, result);
}
/*!
This is the main routine for parsing input on the clientSocket.
There should be one command for each line of input. This reads one
line, and looks at the first word (up to the first space character) to
determine the command. Then if there are body or robot indices to read,
it calls a support routine to read those and return a vector of bodies or
robots. These are then passed to the appropriate routine to carry out the
action and write out any necessary results.
*/
void
ClientSocket::readClient()
{
int i,numData,numBodies,numRobots;
double time;
std::vector<Body *> bodyVec;
std::vector<Robot *> robVec;
bool ok;
while ( canReadLine() ) {
line = readLine();
line.truncate(line.length()-1); //strip newline character
lineStrList =
QStringList::split(' ',line);
strPtr = lineStrList.begin();
#ifdef GRASPITDBG
std::cout <<"Command parser line: "<<line << std::endl;
#endif
if (*strPtr == "getContacts") {
strPtr++; if (strPtr == lineStrList.end()) continue;
numData = (*strPtr).toInt(&ok); strPtr++;
if (!ok) continue;
#ifdef GRASPITDBG
std::cout << "Num data: "<<numData<<std::endl;
#endif
if (readBodyIndList(bodyVec)) continue;
numBodies = bodyVec.size();
for (i=0;i<numBodies;i++)
sendContacts(bodyVec[i],numData);
}
else if (*strPtr == "getAverageContacts") {
strPtr++;
if (readBodyIndList(bodyVec)) continue;
numBodies = bodyVec.size();
for (i=0;i<numBodies;i++)
sendAverageContacts(bodyVec[i]);
}
else if (*strPtr == "getBodyName") {
strPtr++;
if (readBodyIndList(bodyVec)) continue;
numBodies = bodyVec.size();
for (i=0;i<numBodies;i++)
sendBodyName(bodyVec[i]);
}
else if(*strPtr == "setBodyName") {
strPtr++;
int body_index;
if(strPtr != lineStrList.end()){
body_index = strPtr->toInt(&ok);
strPtr++;
if(strPtr == lineStrList.end())
return;
if (body_index == -1 || body_index >= graspItGUI->getIVmgr()->getWorld()->getNumBodies())
{
body_index = graspItGUI->getIVmgr()->getWorld()->getNumBodies() - 1;
}
graspItGUI->getIVmgr()->getWorld()->getBody(body_index)->setName(*strPtr);
}
}
else if (*strPtr == "getRobotName") {
strPtr++;
if (readRobotIndList(robVec)) continue;
numRobots = robVec.size();
for (i=0;i<numRobots;i++)
sendRobotName(robVec[i]);
}
else if (*strPtr == "getDOFVals") {
strPtr++;
if (readRobotIndList(robVec)) continue;
numRobots = robVec.size();
for (i=0;i<numRobots;i++)
sendDOFVals(robVec[i]);
}
else if (*strPtr == "moveDOFs") {
strPtr++;
readDOFVals();
}
else if (*strPtr == "render")
graspItGUI->getIVmgr()->getViewer()->render();
else if (*strPtr == "setDOFForces") {
strPtr++;
if (readRobotIndList(robVec)) continue;
numRobots = robVec.size();
for (i=0;i<numRobots;i++)
if (readDOFForces(robVec[i])==FAILURE) continue;
}
else if ((*strPtr) == "moveDynamicBodies") {
strPtr++;
if (strPtr == lineStrList.end()) ok = FALSE;
else {
time = (*strPtr).toDouble(&ok); strPtr++;
}
if (!ok)
moveDynamicBodies(-1);
else
moveDynamicBodies(time);
}
else if (*strPtr == "computeNewVelocities") {
#ifdef GRASPITDBG
std::cout << "cnv" << std::endl;
#endif
strPtr++; if (strPtr == lineStrList.end()) continue;
time = (*strPtr).toDouble(&ok); strPtr++;
if (!ok) continue;
#ifdef GRASPITDBG
std::cout << time <<std::endl;
#endif
computeNewVelocities(time);
}
else if ((*strPtr) == "outputPlannerResults"){
strPtr++;
outputPlannerResults(0);
}
else if ((*strPtr) == "outputCurrentGrasp"){
strPtr++;
outputCurrentGrasp();
}
else if ((*strPtr) == "sendBodyTransf"){
strPtr++;
verifyInput(1);
sendBodyTransf();
}
else if ((*strPtr) == "setBodyTransf"){
strPtr++;
verifyInput(7);
setBodyTransf();
}
else if ((*strPtr) == "addObstacle"){
strPtr++;
verifyInput(1);
addObstacle(*(strPtr+1));
strPtr+=2;
}
else if ((*strPtr) == "addObject"){
verifyInput(2);
addGraspableBody(*(strPtr+1), *(strPtr+2));
strPtr+=3;
verifyInput(7);
transf object_pose;
readTransf(&object_pose);
World * w = graspItGUI->getIVmgr()->getWorld();
w->getGB(w->getNumGB() - 1)->setTran(object_pose);
}
else if ((*strPtr) == "getCurrentHandTran"){
strPtr++;
getCurrentHandTran();
}
else if ((*strPtr) == "signalGraspUnreachable"){
strPtr+=4;
std::cout << line.toStdString() << std::endl;
graspItGUI->getIVmgr()->blinkBackground();
}
else if ((*strPtr) == "getPlannerTarget"){
strPtr+=1;
QTextStream os (this) ;
os << graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject()->getName() << "\n";
}
else if ((*strPtr) == "setPlannerTarget"){
QTextStream os(this);
os << setPlannerTarget(*(strPtr+1)) << "\n";
strPtr+=2;
}
else if ((*strPtr) == "rotateHandLat"){
strPtr+=1;
rotateHandLat();
}
else if ((*strPtr) == "rotateHandLong"){
strPtr+=1;
rotateHandLong();
}
else if ((*strPtr) == "exec"){
strPtr+=1;
exec();
}
else if ((*strPtr) == "next"){
strPtr+=1;
next();
}
else if ((*strPtr) == "addPointCloud")
{
strPtr += 1;
addPointCloud();
//QTextStream os(this);
//os << addPointCloud() <<" \n";
}
else if ((*strPtr) == "setCameraOrigin")
{
strPtr += 1;
setCameraOrigin();
}
else if ((*strPtr) == "removeBodies"){
strPtr += 1;
removeBodies();
}
else if ((*strPtr) == "clearGraspableBodies"){
strPtr += 1;
removeBodies(true);
}
}
}
//this function is used by the soundstream to output values to the soundcard. This implementation uses
//code given in class though it has been extended.
void testApp::audioOut(float * output,int bufferSize, int nChannels)
{
//if the game is running
if(!gameOver)
{
for(int i=0; i<bufferSize; i++)
{
//use the counter to step from 0 to 1
beat=((int) counter.phasor(16));
//if the beat is different to the previous beat value
if(lastBeat!=beat)
{
//increase the counter and the points
counterVal++;
points++;
//if this is the first beat in a sequence, start all currently created sequences
if(counterVal%16==0)
{
for(int j=0; j<started.size(); j++)
{
started[j]= true;
}
}
//if the counter has gone through a full sequence, increment the number
//of plays for each sequence
if(counterVal%16==15)
{
incrementPlays();
}
//if the counter has reached the next enemy timer
if(counterVal%nextEnemyTime==0)
{
//add an enemy and reset the timer
addCollidable();
nextEnemyTime = rand()%300;
}
//if the counter has reached the next obstacle spawn timer
if(counterVal%nextObstacleTime==0)
{
//add an obstacle and reset the timer
addObstacle();
nextObstacleTime = rand()%500;
}
//if the counter has reached the next powerup spawn timer
if(counterVal%nextPowerupTime==0)
{
//add a powerup and reset the timer
addPowerup();
nextPowerupTime = rand()%700;
}
//trigger sounds to be played
for(int j=0; j<sequences.size(); j++)
{
//for each sequence, if a sound needs to be played in that sequence
if(sequences[j][counterVal%16]==1)
{
//if the sequence is the first sequence, then change the size of the player object
if(j==0)
{
player1.changeBeat();
}
//trigger the sound sample, which resets it's playback position to 0
//the sample to trigger is given by an associated value in a different collection
if(started[j])
{
instTrigger(associatedSound[j]);
}
}
}
}
//update the last beat
lastBeat=beat;
//get the sound value
myOutput = getSound();
//send the output to the soundcard (two values because of stereo)
output[i*nChannels] = myOutput;
output[i*nChannels+1] = myOutput;
}
}
}
