void network()
{
ENetAddress address;
ENetHost* client;
ENetPeer* peer;
std::string message;
ENetEvent event;
int eventStatus;
if (enet_initialize () != 0)
{
std::cout << "Could not start ENet." << std::endl;
return;
}else
std::cout << "Started ENet." << std::endl;
atexit(enet_deinitialize);
client = enet_host_create(NULL, 1, 2, 57600 / 8, 14400 / 8);
enet_address_set_host(&address, "localhost");
char hej[10];
enet_address_get_host(&address, hej, 10);
std::cout << hej << std::endl;
address.port = 1234;
peer = enet_host_connect(client, &address, 2, 0);
while(true)
{
while (enet_host_service(client, &event, 1000) > 0)
{
switch (event.type)
{
case ENET_EVENT_TYPE_CONNECT:
std::cout << "We got a new connection " << event.peer->address.host << std::endl;
break;
case ENET_EVENT_TYPE_RECEIVE:
{
std::cout << "Message from servv: ";
//enet_peer_disconnect(peer, 3);
char header[] = {((char*) event.packet->data)[0],
((char*) event.packet->data)[1],
((char*) event.packet->data)[2],
((char*) event.packet->data)[3]};
int* headerInt = (int*) header;
std::cout << *headerInt << std::endl;
if (*headerInt == PLAYER_POSITION)
{
Package<PLAYER_POSITION_TYPE>* message = (Package<PLAYER_POSITION_TYPE>*) event.packet->data;
std::cout << "player: " << message->_player << std::endl;
std::cout << "x: " << message->_data.x << std::endl;
std::cout << "y: " << message->_data.y << std::endl;
std::cout << "z: " << message->_data.z << std::endl;
}else if (*headerInt == ASSIGN_PLAYER_NUMBER)
{
Package<int>* message = (Package<int>*) event.packet->data;
std::cout << "Player number: " << message->_data << std::endl;
}
// int message = ((char*) event.packet->data)
enet_packet_destroy(event.packet);
break;
}
case ENET_EVENT_TYPE_DISCONNECT:
std::cout << "Disconnected from serv: " << event.peer->data << std::endl;
event.peer->data = NULL;
break;
}
}
posMutex.lock();
glm::vec3 hej = pos;
posMutex.unlock();
// std::cout << "Position sent: (" << hej.x << ", " << hej.y << ", " << hej.x << ")" << std::endl;
ENetPacket* packet = enet_packet_create(&hej, sizeof(glm::vec3), ENET_PACKET_FLAG_RELIABLE);
enet_peer_send(peer, 0, packet);
std::cout << "ping: " << peer->roundTripTime << " ms" << std::endl;
offMutex.lock();
if (turnOfNetwork)
{
offMutex.unlock();
std::cout << "Network recived shoudown command." << std::endl;
break;
}else
offMutex.unlock();
}
std::cout << "Turning of network." << std::endl;
enet_host_destroy(client);
}
void lock()
{ m.lock(); }
std::mutex::native_handle_type primitive()
{ return m.native_handle(); }
int main(){
Uavcam *camera;
bool camera_ok, frame_ok;
int n_saved;
std::stringstream filename;
std::stringstream directory;
std::vector<int> jpg_params;
cv::Mat frame, preview;
parseConfig();
jpg_params.push_back(CV_IMWRITE_JPEG_QUALITY);
jpg_params.push_back(90);
//Set Signals
std::signal(SIGINT,exit_signal); //Set up Ctrl+C signal
//Construct Cameras
if (cameratype == 1){
#ifdef USE_ARAVIS
camera = new AravisCam();
std::cout<<"Using Aravis camera"<<std::endl;
#else
camera = new WebCam();
#endif
}
if(view)
cv::namedWindow("Camera Viewer", cv::WINDOW_AUTOSIZE);
n_saved = checkLog(); //Check the log and open it
openLogtoWrite(n_saved);
ublox = new Gps(); //Initialize the GPS
std::thread gps_thread(gpsUpdate);
camera_ok = camera->initializeCam(); //Initialize the camera
if (camera_ok) {
std::cout << "Start camera acquisition in " << start_delay << " seconds" << std::endl;
std::this_thread::sleep_for(std::chrono::seconds(start_delay));
while(!finish){ //--Main Acquisition Loop
filename.str(""); directory.str(""); //Update filenames
filename<<"im"<<std::setfill('0')<<std::setw(4)<<++n_saved<<".jpg";
directory<<FOLDER<<filename.str();
camera->trigger(); //Send camera trigger
if (usegps){
if(ublox->data_is_good)
std::cout<<"GPS up to date"<<std::endl;
else
std::cout<<"No GPS available" <<std::endl;
}
mtx.lock();
writeImageInfo(ublox->current_loc, filename.str()); //Record GPS
mtx.unlock();
frame_ok = camera->getImage(frame); //Acquire the image
if(frame_ok){ //Acquired Image
cv::resize(frame,preview,cv::Size(),sizefac,sizefac,cv::INTER_NEAREST);
if(saveimg) {
cv::imwrite(directory.str(), preview, jpg_params);
std::cout<<"Saved to " << filename.str() <<std::endl;
}
if(view) {
cv::imshow("Camera Viewer", preview);
cv::waitKey(50);
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
//Finished photographing
delete camera;
}
gps_thread.join();
closeLog();
return 0;
}
int main(int argc, const char* argv[])
{
#ifdef _WIN32
//Start up Winsock…
WSADATA wsadata;
int error = WSAStartup(0x0202, &wsadata);
if (error) //Did something happen?
{
std::cerr<<"Unable to start winsock"<<std::endl;
return -1;
}
if (wsadata.wVersion != 0x0202)//Did we get the right Winsock version?
{
Die("version mismatch");
}
#endif
std::cout << "Enter Server URL: ";
char url[0x100];
std::cin.getline(url,sizeof(url));
std::cout << "Enter your Nickname: ";
char nickname[0x100] = ":";
std::cin.getline(nickname+1,sizeof(nickname)-1);
char*colon = strchr(url,':');
std::string surl,sport;
if (colon != NULL)
{
*colon = 0;
sport = colon+1;
}
else
{
sport = ToString(DEFAULT_PORT);
}
surl = url;
PrintLine("Attempting to connect to "+surl);
ADDRINFOA hints;
memset(&hints,0,sizeof(hints));
hints.ai_protocol = IPPROTO_TCP;
hints.ai_family = AF_UNSPEC;
//hints.
ADDRINFOA*result;
if (getaddrinfo(surl.c_str(),sport.c_str(),&hints,&result) != 0)
{
Die("Call to getaddrinfo() failed");
}
bool connected = false;
while (result) //search through all available results until one allows connection.
//Chances are we get IPv4 and IPv6 results here but the server will only answer to one of those
{
sock = socket(result->ai_family,result->ai_socktype,result->ai_protocol);
if (sock != INVALID_SOCKET) //if we can create a socket then we can attempt a connection. It would be rather unusual for this not to work but...
{
if (!connect(sock,result->ai_addr,result->ai_addrlen)) //attempt connnection
{
//connected
PrintLine("Connected to "+ToString(*result)); //yay, it worked
connected = true;
break;
}
else
{
closesocket(sock); //these aren't the droids we're looking for.
sock = INVALID_SOCKET;
}
}
result = result->ai_next; //moving on.
}
if (!connected)
Die("Failed to connect to "+surl+":"+sport); //so, yeah, none of them worked.
Submit(nickname); //includes leading ':', so that the server knows this is a name, not a message
netThread = std::thread(NetLoop); //start read-thread
while (sock != INVALID_SOCKET)
{
char c = _getch(); //keep reading characters
{
if (c == 3) //this only works on windows, but ctrl-c is handled better on linux anyway
{
Die("Ctrl+C");
break;
}
consoleLock.lock();
if (c == '\n' || c == '\r') //user pressed enter/return:
{
std::string submit = inputBuffer; //copy buffer to string
std::cout << '\r'; //move cursor to line beginning
for (size_t i = 0; i < inputBufferUsage+1; i++) //overwrite line with as many blanks as there were characters
std::cout << ' ';
std::cout << '\r'; //move cursor to line beginning again
inputBufferUsage = 0; //reset character pointer
inputBuffer[0] = 0; //write terminating zero to first char
consoleLock.unlock(); //release console lock
Submit(submit); //process input
}
else
{
if (c == Backspace) //user pressed backspace
{
if (inputBufferUsage > 0)
{
inputBuffer[--inputBufferUsage] = 0; //decrement character pointer and overwrite with 0
std::cout << '\r'<<':'<<inputBuffer<<" \r"<<':'<<inputBuffer;
}
}
else
{
if (c == '!' || c == '?' || ( c != -32 && (isalnum(c) || c == ' '))) //ordinary character
{
if (inputBufferUsage+1 < maxInputBufferUsage) //only allow up to 255 characters though
{
inputBuffer[inputBufferUsage++] = c; //write to the end of the buffer
inputBuffer[inputBufferUsage] = 0; //and terminate properly
}
std::cout << c; //update console
}
}
consoleLock.unlock();
}
}
}
netThread.join();
netThread.detach();
netThread.swap(std::thread());
#ifdef _WIN32
WSACleanup();
#endif
return 0;
}
void gl::globalRelease(bool finish) {
if (finish) {
glFinish();
}
_globalOpenGLLock.unlock();
}
static void log(const std::stringstream &message) {
m.lock();
std::cout << message.str() << std::endl << std::flush;
m.unlock();
}
void CommandReceiver::addCommands(const vector<Command>& commands) {
m_stackLock.lock();
m_commandStack.insert(m_commandStack.end(), commands.begin(), commands.end());
m_stackLock.unlock();
}
void lockScripts()
{
scriptLock.lock();
}
// Worker thread
static void networkThread(void)
{
HttpRequest *request = NULL;
while (true)
{
if (s_need_quit)
{
break;
}
// step 1: send http request if the requestQueue isn't empty
request = NULL;
s_requestQueueMutex.lock();
//Get request task from queue
if (0 != s_requestQueue->count())
{
request = dynamic_cast<HttpRequest*>(s_requestQueue->objectAtIndex(0));
s_requestQueue->removeObjectAtIndex(0);
}
s_requestQueueMutex.unlock();
if (NULL == request)
{
// Wait for http request tasks from main thread
std::unique_lock<std::mutex> lk(s_SleepMutex);
s_SleepCondition.wait(lk);
continue;
}
// step 2: libcurl sync access
// Create a HttpResponse object, the default setting is http access failed
HttpResponse *response = new HttpResponse(request);
// request's refcount = 2 here, it's retained by HttpRespose constructor
request->release();
// ok, refcount = 1 now, only HttpResponse hold it.
int32_t responseCode = -1;
int retValue = 0;
// Process the request -> get response packet
switch (request->getRequestType())
{
case HttpRequest::kHttpGet: // HTTP GET
retValue = processGetTask(request,
writeData,
response->getResponseData(),
&responseCode,
writeHeaderData,
response->getResponseHeader());
break;
case HttpRequest::kHttpPost: // HTTP POST
retValue = processPostTask(request,
writeData,
response->getResponseData(),
&responseCode,
writeHeaderData,
response->getResponseHeader());
break;
case HttpRequest::kHttpPut:
retValue = processPutTask(request,
writeData,
response->getResponseData(),
&responseCode,
writeHeaderData,
response->getResponseHeader());
break;
case HttpRequest::kHttpDelete:
retValue = processDeleteTask(request,
writeData,
response->getResponseData(),
&responseCode,
writeHeaderData,
response->getResponseHeader());
break;
default:
CCASSERT(true, "CCHttpClient: unkown request type, only GET and POSt are supported");
break;
}
// write data to HttpResponse
response->setResponseCode(responseCode);
if (retValue != 0)
{
response->setSucceed(false);
response->setErrorBuffer(s_errorBuffer);
}
else
{
response->setSucceed(true);
}
// add response packet into queue
s_responseQueueMutex.lock();
s_responseQueue->addObject(response);
s_responseQueueMutex.unlock();
// resume dispatcher selector
Director::getInstance()->getScheduler()->resumeTarget(HttpClient::getInstance());
}
// cleanup: if worker thread received quit signal, clean up un-completed request queue
s_requestQueueMutex.lock();
s_requestQueue->removeAllObjects();
s_requestQueueMutex.unlock();
s_asyncRequestCount -= s_requestQueue->count();
if (s_requestQueue != NULL) {
s_requestQueue->release();
s_requestQueue = NULL;
s_responseQueue->release();
s_responseQueue = NULL;
}
}
namespace clog {
std::mutex m;
struct logger2 {
std::stringstream ss;
const char *level;
std::string now()
{
time_t t = time(NULL);
struct tm *tm_info;
tm_info = localtime(&t);
char buf[20];
strftime(buf, 20, "%F %T", tm_info);
return std::string(buf);
}
logger2(const char *llevel)
{
level = llevel;
}
void flush(std::ostream &out)
{
m.lock();
out << level << ": " << now() << " "
<< ss.str() << std::endl;
m.unlock();
}
virtual ~logger2()
{
}
template <typename t>
logger2& operator<<(const t s)
{
ss << s;
return *this;
}
};
/* TODO Copy and pasted code. */
struct err : public logger2 {
#ifdef CPP_LOG_ERR_FILE
std::ofstream ofs;
#endif
err() : logger2("Error") {
#ifdef CPP_LOG_ERR_FILE
ofs.open(CPP_LOG_ERR_FILE,
std::ios_base::app | std::ios_base::out);
#endif
};
~err() {
#ifdef CPP_LOG_ERR_COUT
flush(std::cout);
#endif
#ifdef CPP_LOG_ERR_CERR
flush(std::cerr);
#endif
#ifdef CPP_LOG_ERR_FILE
flush(ofs);
ofs.close();
#endif
}
};
struct warn : public logger2 {
#ifdef CPP_LOG_WARN_FILE
std::ofstream ofs;
#endif
warn() : logger2("Warning") {
#ifdef CPP_LOG_WARN_FILE
ofs.open(CPP_LOG_WARN_FILE,
std::ios_base::app | std::ios_base::out);
#endif
};
~warn() {
#ifdef CPP_LOG_WARN_COUT
flush(std::cout);
#endif
#ifdef CPP_LOG_WARN_CERR
flush(std::cerr);
#endif
#ifdef CPP_LOG_WARN_FILE
flush(ofs);
ofs.close();
#endif
}
};
struct info : public logger2 {
#ifdef CPP_LOG_INFO_FILE
std::ofstream ofs;
#endif
info() : logger2("Info") {
#ifdef CPP_LOG_INFO_FILE
ofs.open(CPP_LOG_INFO_FILE,
std::ios_base::app | std::ios_base::out);
#endif
};
~info() {
#ifdef CPP_LOG_INFO_COUT
flush(std::cout);
#endif
#ifdef CPP_LOG_INFO_CERR
flush(std::cerr);
#endif
#ifdef CPP_LOG_INFO_FILE
flush(ofs);
ofs.close();
#endif
}
};
}
void withLock(std::mutex & m, Function f) {
m.lock();
f();
m.unlock();
}
void smplVec::rarefy(vector<double> depts, string ofile, int rep,
DivEsts* divs, std::vector<vector<rare_map>> & RareSample,
vector<string>& retCntsSampleName, string& skippedSample,
vector<vector<vector<uint>>>* abundInRow, vector<vector<vector<uint>>>* occuencesInRow,
int writes,bool write, bool fillret){
bool doShuffle = true;
long curIdx = 0;
long dep;
// resize divvs
divs->richness.resize(depts.size());
divs->shannon.resize(depts.size());
divs->simpson.resize(depts.size());
divs->invsimpson.resize(depts.size());
divs->chao1.resize(depts.size());
divs->eve.resize(depts.size());
for(uint i = 0; i < depts.size(); i++){
dep = (long)depts[i];
if (dep > totSum){
skippedSample = divs->SampleName;
#ifdef notRpackage
if (verbose){cout<<"skipped sample, because rowSums < depth \n";}
#endif
return;
}
//long curIdx=(long)totSum+1;
for (int curRep=0;curRep<rep;curRep++){
if(curIdx+dep >= (long) totSum || doShuffle == true){
shuffle_singl();
curIdx = 0;
doShuffle = false;
}
//count up
vector<uint> cnts(numFeatures, 0);
for (long i=(0+curIdx);i<(dep+curIdx);i++){
cnts[arr[i]]++;
}
curIdx += dep;
string t_out = ofile;
if (rep!=1){
std::ostringstream oss;
oss<<curRep;
t_out += "_" +oss.str();
}
if (curRep < writes && fillret) {
rare_map cntsMap;
// fill map:
for(uint i = 0; i < cnts.size(); i++){
if(cnts[i] != 0){
cntsMap.insert( std::make_pair(i, cnts[i]) );
}
}
RareSample[i].push_back(cntsMap);
if(curRep == 0){
retCntsSampleName[i] = divs->SampleName; // safe the sample name as well
}
}
richness = 0;
divs->richness[i].push_back(this->getRichness(cnts));
vector<double> three = this->calc_div(cnts, 4);
divs->shannon[i].push_back(three[0]);
divs->simpson[i].push_back(three[1]);
divs->invsimpson[i].push_back(three[2]);
divs->chao1[i].push_back(this->calc_chao1(cnts,1));
divs->eve[i].push_back(this->calc_eveness(cnts));
richness = 0;
// save abundance for chao2 calculations later
rarefyMutex.lock();
for(uint im = 0; im < IDs.size(); im++){
//sparse convertions in swap mode
int id = std::stoi(IDs[im]);
if(cnts[im] != 0){
abundInRow->at(i)[curRep][id]++;
occuencesInRow->at(i)[curRep][id] += cnts[im];
}
}
rarefyMutex.unlock();
}
}
}
void increment(){
mutex.lock();
counter.increment();
mutex.unlock();
}
void add_inj(int other) {
mutex.lock();
inj.push_back(other);
mutex.unlock();
}
void unlockScripts()
{
scriptLock.unlock();
}
void gl::globalLock() {
_globalOpenGLLock.lock();
}
namespace bch {
class alignas(4) Test
{
public:
Test()
{
#if BCH_SMART_PTR_UNITTEST
std::cout << "Test ctor(1)" << std::endl;
#endif
Fill(1);
}
explicit Test(int v)
{
#if BCH_SMART_PTR_UNITTEST
std::cout << "Test ctor(2)" << std::endl;
#endif
Fill(v);
}
~Test()
{
#if BCH_SMART_PTR_UNITTEST
std::cout << "Test dtor" << std::endl;
#endif
}
void Baz()
{
}
private:
void Fill(int v)
{
for (int i = 0; i < sizeof(mValue)/sizeof(*mValue); ++i)
mValue[i] = v++;
}
char mValue[4];
};
class TestDer: public Test
{
public:
TestDer()
{
#if BCH_SMART_PTR_UNITTEST
std::cout << "TestDer ctor" << std::endl;
#endif
}
~TestDer()
{
#if BCH_SMART_PTR_UNITTEST
std::cout << "TestDer dtor" << std::endl;
#endif
}
};
template <typename T>
void Foo(T value)
{
value->Baz();
}
const unsigned int kTestCount = 100000000;
// const unsigned int kTestCount = 1;
std::mutex mutex;
double stdPtrTime = 0;
double noThreadTime = 0;
void std_poiner()
{
typedef std::chrono::time_point<std::chrono::system_clock> TimerType;
TimerType start = std::chrono::system_clock::now();
std::shared_ptr<Test> stdValue(new Test);
for (unsigned int i = 0; i < kTestCount; ++i)
{
Foo(stdValue);
}
TimerType end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end-start;
double elapsed = elapsed_seconds.count();;
{
mutex.lock();
stdPtrTime += elapsed;
std::cout << "Time with std::shared_ptr is " << elapsed << std::endl;
mutex.unlock();
}
}
void nothread_poiner()
{
typedef std::chrono::time_point<std::chrono::system_clock> TimerType;
TimerType start = std::chrono::system_clock::now();
shared_ptr_nc<Test> stdValue(new Test);
for (unsigned int i = 0; i < kTestCount; ++i)
{
Foo(stdValue);
}
TimerType end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end-start;
double elapsed = elapsed_seconds.count();
{
mutex.lock();
noThreadTime += elapsed;
std::cout << "Time with no-thread::shared_ptr_nc is " << elapsed << std::endl;
mutex.unlock();
}
}
class SizeTest
{
char x;
};
} // namespace bch
void func(int i) {
mutex.lock();
std::cout << "func clled, i = " << i << " /" << std::this_thread::get_id() << std::endl;
mutex.unlock();
}
// Will analyse every buffer from
void detection(uint nDetect, int nStorage) {
bool isDetected=false;
int nCurrent=0;
short *storage_short;
storage_short=new short [nStorage];
short *p=0;
float power;
int i2=0,count_trans=0;
std::vector<std::thread> processingV;
int count=0;
while(1) {
std::this_thread::yield();
if (isDetected) {
// Store the elements in storage_short
if (nCurrent<nStorage) {
// Fill storage_short
i2=0;
int conj_rx;
while ((nCurrent<nStorage) && (i2<2*((int) nDetect))) {
conj_rx=pow(-1,nCurrent);
//DispVal(conj_rx);
storage_short[nCurrent]=(p[i2])*conj_rx;
nCurrent++;
i2++;
};
// Relase the just stored element
std::this_thread::yield();
delete[] (usrpQ.front());
mtxUsrp.lock();
usrpQ.pop();
mtxUsrp.unlock();
std::this_thread::yield();
//Wait for a new element
sem_wait(&usrpReady);
p=usrpQ.front();
}
else {
// End of transmission
//count_trans=0;
isDetected=false;
nCurrent=0;
processingV.push_back(std::thread(processing, storage_short, nStorage, count));
count++;
storage_short=new short[nStorage];
}
}
else {
// Wait for new element
sem_wait(&usrpReady);
p=usrpQ.front();
// Test of detection
power=powerTotArray(p, (int)2*nDetect);
//if(power>50000)DispVal(power);
count_trans++;//Avoid transient power amplification
//Power threshold 60GHz=1000
if ( power>600 && count_trans>10000 && count<10) {
// If detection, keep the element
std::cout<<"Detected Transmission\n";
//exit(1);
isDetected=true;
}
else {
// Release the element
std::this_thread::yield();
delete[] (usrpQ.front());
mtxUsrp.lock();
usrpQ.pop();
mtxUsrp.unlock();
std::this_thread::yield();
}
}
}
for (auto& th : processingV) th.join();
}
void exit_signal(int param){
mtx.lock();
finish = 1;
std::cout << "Exiting on next frame completion.." << std::endl;
mtx.unlock();
}
void cloudViewer()
{
ros::Publisher pub = nh.advertise<pcl::PointCloud<pcl::PointXYZRGBA> > ("/original_pointcloud", 100);
//######################################################################################
// if you want to visualize original pc uncomment the following
//######################################################################################
// cv::Mat color, depth;
// pcl::visualization::PCLVisualizer::Ptr visualizer(new pcl::visualization::PCLVisualizer("Cloud Viewer"));
// const std::string cloudName = "rendered";
//
// lock.lock();
// color = this->color;
// depth = this->depth;
// updateCloud = false;
// lock.unlock();
//
// createCloud(depth, color, cloud);
//
// visualizer->addPointCloud(cloud, cloudName);
// visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloudName);
// visualizer->initCameraParameters();
// visualizer->setBackgroundColor(0, 0, 0);
// visualizer->setPosition(mode == BOTH ? color.cols : 0, 0);
// visualizer->setSize(color.cols, color.rows);
// visualizer->setShowFPS(true);
// visualizer->setCameraPosition(0, 0, 0, 0, -1, 0);
// // visualizer->registerKeyboardCallback(&Receiver::keyboardEvent, *this);
//######################################################################################
for(; running && ros::ok();)
{
if(updateCloud)
{
lock.lock();
color = this->color;
depth = this->depth;
updateCloud = false;
lock.unlock();
createCloud(depth, color, cloud);
//######################################################################################
// visualizer->updatePointCloud(cloud, cloudName);
//######################################################################################
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr msg (new pcl::PointCloud<pcl::PointXYZRGBA>);
msg->header.frame_id = "kinect2_rgb_optical_frame";
*msg += *cloud;
msg->header.stamp = ros::Time::now().toNSec();
pub.publish(msg);
}
// if(save)
// {
// save = false;
// cv::Mat depthDisp;
// dispDepth(depth, depthDisp, 12000.0f);
// saveCloudAndImages(cloud, color, depth, depthDisp);
// }
//######################################################################################
// visualizer->spinOnce(10);
//######################################################################################
}
//######################################################################################
// visualizer->close();
//######################################################################################
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "pathplanner");
ros::NodeHandle nh;
ros::Subscriber map_sub = nh.subscribe("/map", 1, map_callback);
ros::Subscriber pose_sub = nh.subscribe("/odometry/filtered", 1, position_callback);
ros::Subscriber waypoint_sub = nh.subscribe("/waypoint", 1, waypoint_callback);
disp_path_pub = nh.advertise<nav_msgs::Path>("/path_display", 1);
act_path_pub = nh.advertise<igvc_msgs::action_path>("/path", 1);
expanded_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/expanded", 1);
path_planner_map_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/path_planner_incremental", 1);
double baseline = 0.93;
ros::NodeHandle pNh("~");
search_problem.Map = pcl::PointCloud<pcl::PointXYZ>().makeShared();
search_problem.Map->header.frame_id = "/odom";
search_problem.Octree = boost::make_shared<pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>>(0.1);
search_problem.Octree->setInputCloud(search_problem.Map);
if (!pNh.hasParam("goal_threshold") || !pNh.hasParam("threshold") || !pNh.hasParam("speed") ||
!pNh.hasParam("baseline") || !pNh.hasParam("minimum_omega") || !pNh.hasParam("maximum_omega") ||
!pNh.hasParam("delta_omega") || !pNh.hasParam("point_turns_enabled") || !pNh.hasParam("reverse_enabled") ||
!pNh.hasParam("max_obstacle_delta_t") || !pNh.hasParam("alpha") || !pNh.hasParam("beta") ||
!pNh.hasParam("bounding_distance"))
{
ROS_ERROR_STREAM("path planner does not have all required parameters");
return 0;
}
pNh.getParam("goal_threshold", search_problem.GoalThreshold);
pNh.getParam("threshold", search_problem.Threshold);
pNh.getParam("speed", search_problem.Speed);
pNh.getParam("baseline", search_problem.Baseline);
search_problem.DeltaT = [](double distToStart, double distToGoal) -> double {
return -((distToStart + distToGoal) / 7 / (pow((distToStart + distToGoal) / 2, 2)) *
pow(distToStart - (distToStart + distToGoal) / 2, 2)) +
(distToStart + distToGoal) / 7 + 0.3;
};
pNh.getParam("minimum_omega", search_problem.MinimumOmega);
pNh.getParam("maximum_omega", search_problem.MaximumOmega);
pNh.getParam("delta_omega", search_problem.DeltaOmega);
pNh.getParam("point_turns_enabled", search_problem.PointTurnsEnabled);
pNh.getParam("reverse_enabled", search_problem.ReverseEnabled);
pNh.getParam("max_obstacle_delta_t", search_problem.MaxObstacleDeltaT);
pNh.getParam("alpha", search_problem.Alpha);
pNh.getParam("beta", search_problem.Beta);
pNh.getParam("bounding_distance", search_problem.BoundingDistance);
ros::Rate rate(3);
while (ros::ok())
{
ros::spinOnce();
/* Do not attempt to plan a path if the path length would be greater than 100ft (~30m).
* This should only happen when we have received either a waypoint or position estimate, but not both.
* Long paths take forever to compute, and will freeze up this node.
*/
auto distance_to_goal = search_problem.Start.distTo(search_problem.Goal);
if (!received_waypoint || distance_to_goal == 0 || distance_to_goal > 60)
continue;
planning_mutex.lock();
Path<SearchLocation, SearchMove> path;
path = GraphSearch::AStar(search_problem, expanded_callback);
if (act_path_pub.getNumSubscribers() > 0)
{
nav_msgs::Path disp_path_msg;
disp_path_msg.header.stamp = ros::Time::now();
disp_path_msg.header.frame_id = "odom";
if (path.getStates()->empty())
path.getStates()->push_back(search_problem.Start);
for (auto loc : *(path.getStates()))
{
geometry_msgs::PoseStamped pose;
pose.header.stamp = disp_path_msg.header.stamp;
pose.header.frame_id = disp_path_msg.header.frame_id;
pose.pose.position.x = loc.x;
pose.pose.position.y = loc.y;
disp_path_msg.poses.push_back(pose);
}
disp_path_pub.publish(disp_path_msg);
igvc_msgs::action_path act_path_msg;
act_path_msg.header.stamp = ros::Time::now();
act_path_msg.header.frame_id = "odom";
for (auto action : *(path.getActions()))
{
igvc_msgs::velocity_pair vels;
vels.header.stamp = act_path_msg.header.stamp;
vels.header.frame_id = act_path_msg.header.frame_id;
if (action.W != 0)
{
double radius = action.V / action.W;
vels.right_velocity = (radius - baseline / 2.) * action.W;
vels.left_velocity = (radius + baseline / 2.) * action.W;
}
else
{
vels.right_velocity = 1.0;
vels.left_velocity = 1.0;
}
vels.duration = action.DeltaT;
act_path_msg.actions.push_back(vels);
}
act_path_pub.publish(act_path_msg);
}
planning_mutex.unlock();
rate.sleep();
}
return 0;
}
void elog::setCallbackLog(const elog::callbackLog& _callback) {
// TODO : Check atomicity ...
g_lock.lock();
callbackUserLog = _callback;
g_lock.unlock();
}
//发送数据线程
void netSocketManger::sendThread()
{
//bool connect = cSocket.Connect(kServerIP,kServerPort);
bool connect = cSocket.Connect(netSocketManger::sharednetSocketManger()->server.IP.c_str(),netSocketManger::sharednetSocketManger()->server.port);
// cSocket.Send("bbb",strlen("bbb")+1,0);
if (connect) {
isConnect = true;
SocketData *errorData = newSocketData();
errorData->eventType = CONNECT_SUCCEED;
m_mainQueue.Push(errorData);
//创建接收线程
m_mutexx.lock();
netSocketManger::sharednetSocketManger()->createReciveThread();
m_mutexx.unlock();
m_sendEvent->Lock();
while (true) {
while (!m_sendQueue.IsEmpty()) {
SocketData *data = m_sendQueue.Pop();
uLong comprLen = data->bodyLen;
const char *compr = data->sendData;
T_MSGHEAD_T msgHead;
msgHead.cmd = (unsigned short)data->module.cmd;
msgHead.com = 0;
msgHead.enc = 0;
msgHead.eno = 0;
msgHead.idx = (uInt)data->sn;
msgHead.len = (unsigned short)comprLen;
msgHead.tea = 0;//s_tea;
unsigned char *sendData = (unsigned char*)malloc(comprLen);
unsigned int pos = 0;
memcpy(&sendData[pos], compr, comprLen);//body
pos += comprLen;
int ret = cSocket.Send((char*)sendData,pos,0);
log("发送:%s",compr);
if (ret <= 0) {
m_sendEvent->Unlock();
free(sendData);
SocketData *errorData = newSocketData();
errorData->eventType = DISCONNECT;
m_mainQueue.Push(errorData);
return;
}
free(data->sendData);
free(data);
free(sendData);
log("-----发送数据长度len:%d------",msgHead.len);
log("-----------");
}
m_sendEvent->Wait();
}
m_sendEvent->Unlock();
}else {
isConnect = false;
SocketData *errorData = newSocketData();
errorData->eventType = CONNECT_FAIL;
m_mainQueue.Push(errorData);
}
}
void elog::logChar(int32_t _id, int32_t _level, int32_t _ligne, const char* _funcName, const char* _log) {
// special callback mode:
if (callbackUserLog != nullptr) {
const char* libName = "";
if (_id >= 0) {
libName = getList()[_id].first.c_str();
}
g_lock.lock();
if (callbackUserLog != nullptr) {
callbackUserLog(libName, elog::level(_level), _ligne, _funcName, _log);
}
g_lock.unlock();
return;
}
char handle[LENGHT_MAX_LOG] = "";
memset(handle, ' ', LENGHT_MAX_LOG);
handle[0] = '\0';
char* pointer = handle;
if(getColor() == true) {
switch(_level) {
default:
// nothing to do ...
break;
case elog::level_critical:
strcat(pointer, ETK_BASH_COLOR_BOLD_RED);
break;
case elog::level_error:
strcat(pointer, ETK_BASH_COLOR_RED);
break;
case elog::level_warning:
strcat(pointer, ETK_BASH_COLOR_MAGENTA);
break;
case elog::level_info:
strcat(pointer, ETK_BASH_COLOR_CYAN);
break;
case elog::level_debug:
strcat(pointer, ETK_BASH_COLOR_YELLOW);
break;
case elog::level_verbose:
strcat(pointer, ETK_BASH_COLOR_WHITE);
break;
case elog::level_print:
strcat(pointer, ETK_BASH_COLOR_WHITE);
break;
}
pointer = handle+strlen(handle);
}
if(getTime() == true) {
getDisplayTime(pointer);
pointer = handle+strlen(handle);
}
#ifndef __TARGET_OS__Android
switch(_level) {
default:
strcat(pointer, "[?] ");
break;
case elog::level_print:
strcat(pointer, "[P] ");
break;
case elog::level_critical:
strcat(pointer, "[C] ");
break;
case elog::level_error:
strcat(pointer, "[E] ");
break;
case elog::level_warning:
strcat(pointer, "[W] ");
break;
case elog::level_info:
strcat(pointer, "[I] ");
break;
case elog::level_debug:
strcat(pointer, "[D] ");
break;
case elog::level_verbose:
strcat(pointer, "[V] ");
break;
}
pointer = handle+strlen(handle);
#endif
if (getLibName() == true) {
if (_id >= 0) {
int32_t len = strlen(handle);
strcat(pointer, getList()[_id].first.c_str());
pointer = handle+strlen(handle);
while (strlen(handle) - len < getNameSizeLog()) {
*pointer++ = ' ';
*pointer = '\0';
}
*pointer++ = '|';
*pointer++ = ' ';
*pointer = '\0';
}
}
#ifdef ELOG_BUILD_ETHREAD
if(getThreadId() == true) {
// display thread ID
uint32_t iddd = ethread::getId();
sprintf(pointer, "%3d", iddd);
pointer = handle+strlen(handle);
*pointer++ = ' ';
*pointer++ = '|';
*pointer++ = ' ';
*pointer = '\0';
}
if(getThreadNameEnable() == true) {
// display thread ID
std::string name = ethread::getName();
if (name.size() >= getThreadSizeLog() ) {
getThreadSizeLog() = name.size() + 1;
}
sprintf(pointer, "%s", name.c_str());
pointer = handle+strlen(handle);
size_t nbSpaceToAdd = getThreadSizeLog()-name.size();
for (size_t iii=0; iii<nbSpaceToAdd; ++iii) {
*pointer++ = ' ';
*pointer = '\0';
}
*pointer++ = '|';
*pointer++ = ' ';
*pointer = '\0';
}
#endif
if(getLine() == true) {
if (_ligne >= 0) {
sprintf(pointer, "(l=%5d)", _ligne);
pointer = handle+strlen(handle);
*pointer++ = ' ';
*pointer = '\0';
}
}
// TODO :Maybe optimize this one ...
if(getFunction() == true) {
int32_t len = strlen(handle);
char tmpName[1024];
char *tmpPointer = tmpName;
if (_funcName != nullptr) {
// cleen for android :
char* startPos = strchr((char*)_funcName, ' ');
char* stopPos = strchr((char*)_funcName, '(');
if (startPos != nullptr) {
if (stopPos != nullptr) {
char* startPos2 = strchr(startPos+1, ' ');
while ( startPos2 != nullptr
&& startPos2 < stopPos) {
startPos = startPos2;
startPos2 = strchr(startPos+1, ' ');
}
if(uint64_t(stopPos) < uint64_t(startPos)) {
snprintf(tmpPointer, std::min(uint64_t(1024), uint64_t(stopPos)-uint64_t(_funcName)), "%s", _funcName);
} else {
snprintf(tmpPointer, std::min(uint64_t(1024), uint64_t(stopPos)-uint64_t(startPos)), "%s", startPos+1);
}
} else {
snprintf(tmpPointer, 1024, "%s", startPos);
}
} else {
if (stopPos != nullptr) {
snprintf(tmpPointer, std::min(uint64_t(1024), uint64_t(stopPos)-uint64_t(_funcName)+1), "%s", _funcName);
} else {
snprintf(tmpPointer, 1024, "%s", _funcName);
}
}
tmpPointer = tmpPointer+strlen(tmpPointer);
}
size_t lenFunc = strlen(tmpName);
if (lenFunc >= getFunctionSizeLog()) {
getFunctionSizeLog() = lenFunc+1;
}
size_t nbSpaceToAdd = getFunctionSizeLog() - lenFunc;
for (size_t iii=0; iii<nbSpaceToAdd; ++iii) {
*tmpPointer++ = ' ';
*tmpPointer = '\0';
}
*tmpPointer++ = '|';
*tmpPointer++ = ' ';
*tmpPointer = '\0';
strcat(pointer, tmpName);
pointer += strlen(tmpName);
}
if (strlen(_log) > LENGHT_MAX_LOG - strlen(handle)-20) {
memcpy(pointer, _log, LENGHT_MAX_LOG - strlen(handle)-21);
handle[1024-25] = ' ';
handle[1024-24] = '.';
handle[1024-23] = '.';
handle[1024-22] = '.';
handle[1024-21] = '\0';
} else {
strcat(pointer, _log);
}
pointer = handle+strlen(handle);
if(getColor() == true) {
strcat(pointer, ETK_BASH_COLOR_NORMAL);
}
g_lock.lock();
{
FILE*& file = getLogFile();
// close file only if needed ...
if (file != nullptr) {
*pointer++ = '\n';
*pointer = '\0';
fprintf(file, handle);
switch(_level) {
default:
break;
case elog::level_critical:
case elog::level_error:
fflush(file);
break;
}
// if we log in file, we have no need to log otherwise ... just "tail -f log.txt"
g_lock.unlock();
return;
}
}
#if defined(__TARGET_OS__Android)
switch(_level) {
default:
__android_log_print(ANDROID_LOG_VERBOSE, "EWOL", "%s", handle);
break;
case elog::level_print:
__android_log_print(ANDROID_LOG_INFO, "EWOL", "%s", handle);
break;
case elog::level_critical:
__android_log_print(ANDROID_LOG_FATAL, "EWOL", "%s", handle);
break;
case elog::level_error:
__android_log_print(ANDROID_LOG_ERROR, "EWOL", "%s", handle);
break;
case elog::level_warning:
__android_log_print(ANDROID_LOG_WARN, "EWOL", "%s", handle);
break;
case elog::level_info:
__android_log_print(ANDROID_LOG_INFO, "EWOL", "%s", handle);
break;
case elog::level_debug:
__android_log_print(ANDROID_LOG_DEBUG, "EWOL", "%s", handle);
break;
case elog::level_verbose:
__android_log_print(ANDROID_LOG_VERBOSE, "EWOL", "%s", handle);
break;
}
#elif defined(__TARGET_OS__IOs)
iosNSLog(handle);
#else
std::cout << handle << std::endl;
#endif
g_lock.unlock();
if (_level == level_critical) {
std::this_thread::sleep_for(std::chrono::milliseconds(700));
displayBacktrace(true, 2);
}
// Display backtrace to facilitate the error problems
if ( _level == level_error
&& getDisplayBackTrace() == true) {
displayBacktrace(false, 2);
}
}
void unlock()
{ m.unlock(); }
void add_adj(int other) {
mutex.lock();
adj.push_back(other);
mutex.unlock();
}
namespace BB
{
// Global variable for now
std::mutex boundTightenerMutex;
OBBT::OBBT(double threshold, unsigned int maxIterations)
: BoundsTightener(threshold, maxIterations),
doParallelComputing(false),
doAggressiveBoundsTightening(false)
{
}
bool OBBT::doTightening(ConstraintPtr constraints)
{
// Get convex relaxation
ConstraintPtr convexConstraints = constraints->getConvexRelaxation();
assert(convexConstraints != nullptr);
assert(convexConstraints->isConstraintConvex());
// Tighten bounds of all complicating variables
std::vector<VariablePtr> variables;
for (auto var : constraints->getComplicatingVariables())
{
if (assertNear(var->getUpperBound(), var->getLowerBound()))
continue;
variables.push_back(var);
}
// Check if there are any variables to tighten
if (variables.size() < 1)
return true;
// Tighten bounds
return tightenBoundsSequential(convexConstraints, variables);
// bool success = true;
// if (doParallelComputing)
// {
// tightenBoundsParallel(convexConstraints, variables);
// }
// else
// {
// success = tightenBoundsSequential(convexConstraints, variables);
// }
// return success;
}
bool OBBT::tightenBoundsSequential(ConstraintPtr cs, std::vector<VariablePtr> variables)
{
for (auto var : variables)
{
// Tighten bounds of single variable
if (!tightenVariableBound(cs, var))
return false;
}
return true;
}
void OBBT::tightenBoundsParallel(ConstraintPtr cs, std::vector<VariablePtr> variables)
{
assert(variables.size() > 0);
std::vector<std::thread> threads;
int numThreads = std::thread::hardware_concurrency();
if (numThreads < 1) numThreads = 1;
int varsPerThread = variables.size()/numThreads;
// printVec(variables);
// cout << "Variables: " << variables.size() << endl;
// cout << "Threads: " << numThreads << endl;
for (int i = 0; i < numThreads; i++)
{
int start = i*varsPerThread;
int end = (i+1)*varsPerThread;
if (i == numThreads - 1) end = variables.size();
//cout << "Thread " << i << ": " << start << " to " << end << endl;
std::vector<VariablePtr>::const_iterator varStart = variables.begin() + start;
std::vector<VariablePtr>::const_iterator varEnd = variables.begin() + end;
std::vector<VariablePtr> threadVariables(varStart, varEnd);
//printVec(threadVariables);
threads.push_back(std::thread(&OBBT::tightenVariableBounds, this, cs, threadVariables));
// threads.push_back(thread(&BoundTightener::test, this, convexConstraints, z0_aug, ref(threadVariables)));
}
for (int i = 0; i < numThreads; i++)
{
threads.at(i).join();
}
}
void OBBT::tightenVariableBounds(ConstraintPtr cs, std::vector<VariablePtr> variables)
{
for (auto var : variables)
{
// Read variable bounds
boundTightenerMutex.lock();
tightenVariableBound(cs, var);
boundTightenerMutex.unlock();
}
}
bool OBBT::tightenVariableBound(ConstraintPtr cs, VariablePtr variable)
{
assert(cs->hasVariable(variable));
auto vars = cs->getVariables();
// Store and set objective costs to zero
std::vector<double> costs;
for (auto var : vars)
{
costs.push_back(var->getCost());
}
// Set costs for lower bound problem
for (auto var : vars)
{
if (var == variable)
var->setCost(1); // Min. variable
else
var->setCost(0);
}
//SolverIpopt solver_min(cs);
SolverGurobi solver_min(cs);
SolverResult result_min = solver_min.optimize();
// Set costs for upper bound problem
for (auto var : vars)
{
if (var == variable)
var->setCost(-1); // Max. variable
else
var->setCost(0);
}
//SolverIpopt solver_max(cs);
SolverGurobi solver_max(cs);
SolverResult result_max = solver_max.optimize();
// Reset costs
int counter = 0;
for (auto var : vars)
{
var->setCost(costs.at(counter));
counter++;
}
// Check for infeasibility
if (result_min.status == SolverStatus::INFEASIBLE
|| result_max.status == SolverStatus::INFEASIBLE)
return false;
// Update lower bound
if (result_min.status == SolverStatus::OPTIMAL)
{
if (!variable->updateLowerBound(result_min.objectiveValue))
{
// This should never happen!
cout << "Min bound" << endl;
cout << *variable << endl;
cout << result_min << endl;
return false;
}
}
// Update upper bound
if (result_max.status == SolverStatus::OPTIMAL)
{
if (!variable->updateUpperBound(-result_max.objectiveValue))
{
cout << std::setprecision(10) << -result_max.objectiveValue << endl;
cout << std::setprecision(10) << variable->getLowerBound() << endl;
cout << std::setprecision(10) << variable->getLowerBound() + result_max.objectiveValue << endl;
// This should never happen!
cout << "Max bound" << endl;
cout << *variable << endl;
cout << result_max << endl;
return false;
}
}
// No update
return true;
}
//void FBBT::tightenBoundsAggressive(ConstraintPtr cs,
// std::vector<double> z0,
// std::vector<int> variables,
// std::vector<double> &lb,
// std::vector<double> &ub)
//{
// cout << "Performing aggressive bound tightening..." << endl;
// // z0 is assumed to be the lower bound solution
// assert(variables.size() > 0);
// for (unsigned int i = 0; i < variables.size(); i++)
// {
// int var = variables.at(i);
// double varopt = z0.at(var);
// double varlb = lb.at(var);
// double varub = ub.at(var);
// double dist = varub - varlb;
// //varopt = (varub + varlb)/2.0;
// if (dist < 0.1) continue;
// cout << "varlb: " << varlb << endl;
// cout << "varlb: " << varub << endl;
// cout << "var: " << varopt << endl;
// // Try to update lower bound by introducing a fake upper bound
// double ratio = 2.0/3.0; // 0 < ratio < 1
// double fakeub = varlb + ratio*(varopt - varlb);
// if ((fakeub - varlb)/dist > 0.1)
// {
// ConstraintPtr cs_copy(cs->clone());
// std::vector<double> lb_copy;
// std::vector<double> ub_copy;
// cs_copy->getDomainBounds(lb_copy,ub_copy);
// ub_copy.at(var) = fakeub;
// cs_copy->setDomainBounds(lb_copy, ub_copy);
// // Solve for new lower bound on variable
// DenseMatrix cmin; cmin.setZero(1,z0.size()); cmin(var) = 1;
// ObjectivePtr minObj(new ObjectiveLinear(cmin));
// //SolverIpopt ip_min(minObj, cs_copy, z0);
// SolverGurobi solver_min(minObj, cs_copy, z0);
// solver_min.initialize();
// int status_min = solver_min.optimize();
// if (status_min != 1)
// {
// cout << "Able to tighten lower bound by aggressive FBBT!" << endl;
// cout << "old lb: " << lb.at(var) << endl;
// cout << "new lb: " << fakeub << endl;
// cout << "ub: " << ub.at(var) << endl;
// lb.at(var) = fakeub;
// varlb = fakeub;
// dist = varub - varlb;
// }
// }
// // Try to update upper bound by introducing a fake lower bound
// double fakelb = varub - ratio*(varub - varopt);
// if ((varub - fakelb)/dist > 0.1)
// {
// ConstraintPtr cs_copy(cs->clone());
// std::vector<double> lb_copy;
// std::vector<double> ub_copy;
// cs_copy->getDomainBounds(lb_copy,ub_copy);
// lb_copy.at(var) = fakelb;
// cs_copy->setDomainBounds(lb_copy, ub_copy);
// // Solve for new lower bound on variable
// DenseMatrix cmin; cmin.setZero(1,z0.size()); cmin(var) = 1;
// ObjectivePtr minObj(new ObjectiveLinear(cmin));
// //SolverIpopt ip_min(minObj, cs_copy, z0);
// SolverGurobi solver_min(minObj, cs_copy, z0);
// solver_min.initialize();
// int status_min = solver_min.optimize();
// if (status_min != 1)
// {
// cout << "Able to tighten upper bound by aggressive FBBT!" << endl;
// ub.at(var) = fakelb;
// }
// }
// }
//}
} // namespace BB
void myPreSyncFun()
{
if( gEngine->isMaster() )
{
// if( mouseLeftButton )
// {
//double tmpYPos;
//get the mouse pos from first window
sgct::Engine::getMousePos( gEngine->getFocusedWindowIndex(), &mouseXPos[0], &mouseYPos[0] );
// mouseDx = mouseXPos[0] - mouseXPos[1];
// mouseDy = mouseYPos[0] - mouseYPos[1];
mouseDx = mouseXPos[0] - 960/2;
mouseDy = mouseYPos[0] - 540/2;
// }
// else
// {
// mouseDx = 0.0;
// mouseDy = 0.0;
// }
sgct::Engine::setMousePos( gEngine->getFocusedWindowIndex(), 960/2, 540/2);
static float panRot = 0.0f;
panRot += (static_cast<float>(mouseDx) * rotationSpeed * static_cast<float>(gEngine->getDt()));
static float vertRot = 0.0f;
vertRot += (static_cast<float>(mouseDy) * rotationSpeed * static_cast<float>(gEngine->getDt()));
glm::mat4 ViewRotateX = glm::rotate(
glm::mat4(1.0f),
panRot,
glm::vec3(0.0f, 1.0f, 0.0f)); //rotation around the y-axis
glm::mat4 ViewRotateY = glm::rotate(
glm::mat4(1.0f),
vertRot,
glm::vec3(1.0f, 0.0f, 0.0f)); //rotation around the x-axis
view = glm::inverse(glm::mat3(ViewRotateX)) * glm::vec3(0.0f, 0.0f, 1.0f);
glm::vec3 right = glm::cross(view, up);
posMutex.lock();
if( arrowButtons[FORWARD] )
pos += (walkingSpeed * static_cast<float>(gEngine->getDt()) * view);
if( arrowButtons[BACKWARD] )
pos -= (walkingSpeed * static_cast<float>(gEngine->getDt()) * view);
if( arrowButtons[LEFT] )
pos -= (walkingSpeed * static_cast<float>(gEngine->getDt()) * right);
if( arrowButtons[RIGHT] )
pos += (walkingSpeed * static_cast<float>(gEngine->getDt()) * right);
posMutex.unlock();
/*
To get a first person camera, the world needs
to be transformed around the users head.
This is done by:
1, Transform the user to coordinate system origin
2, Apply transformation
3, Transform the user back to original position
However, mathwise this process need to be reversed
due to the matrix multiplication order.
*/
// std::cout << "Position: (" << pos.x << ", " << pos.y << ", " << pos.x << ")" << std::endl;
//3. transform user back to original position
glm::mat4 result;
result = glm::translate( glm::mat4(1.0f), sgct::Engine::getUserPtr()->getPos() );
//2. apply transformation
result *= (ViewRotateY * ViewRotateX * glm::translate( glm::mat4(1.0f), pos ));
//1. transform user to coordinate system origin
result *= glm::translate( glm::mat4(1.0f), -sgct::Engine::getUserPtr()->getPos() );
xform.setVal( result );
}
}