RenderingParameters.cpp

/* ----------------------
* Copyright 2023 Université Libre de Bruxelles(ULB), Universidad Politécnica de Madrid(UPM), CREAL, Deutsches Zentrum für Luft - und Raumfahrt(DLR)
 
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at < http://www.apache.org/licenses/LICENSE-2.0>
 
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissionsand
* limitations under the License.
---------------------- */
 
 
#include "RenderingParameters.h"
#include"../WindowAbstract.h"
 
#include "VulkanHelperFunction.h"
#include "VulkanWrapper.h"
 
#ifdef  HVT_UDP_CONTROL
void RenderingParameters::init(int viewNum, WindowAbstract* wind, VulkanWrapper* wraps, unsigned short portNumber)
{
    this->portNumber = portNumber;
#else
void RenderingParameters::init(int viewNum, WindowAbstract * wind,  VulkanWrapper* wraps)
{
#endif
    window = wind;
    wrapper = wraps;
 
    virtualExtrinsics.resize(viewNum);
    virtualIntrasics.resize(viewNum);
    virtualViewDef.resize(viewNum);
 
    staticPosition.resize(staticPoses);
    staticQuaternion.resize(staticPoses);
 
    for (int i = 0; i < viewNum; i++) {
        auto ext = window->getSwapchainExtent(i);
        virtualViewDef[i] = glm::vec2(ext.width, ext.height);
        virtualExtrinsics[i].position = glm::vec3(0, 0, 0);
        virtualExtrinsics[i].rotation = glm::vec3(0, 0, 0);
        projectionType = InputProvider::ProjectionType::PROJECTION_PERSPECTIVE;
        auto intra = InputProvider::PerspectiveIntrinsics();
        virtualIntrasics[i] = intra;
        std::get<InputProvider::PerspectiveIntrinsics>(virtualIntrasics[i]).principle = glm::vec2(ext.width, ext.height) / 2.0f;
        std::get<InputProvider::PerspectiveIntrinsics>(virtualIntrasics[i]).focals = glm::vec2(ext.width, ext.width) / 2.0f;
        virtualIntrasics.push_back(intra);
    }
#ifdef HVT_UDP_CONTROL
    setupUDPServer();
#endif // HVT_UDP_CONTROL
 
}
 
RenderingParameters::~RenderingParameters()
{
#ifdef HVT_UDP_CONTROL
    serverIsActive = false;
    serverThread.join();
#endif
}
 
void RenderingParameters::updateSpaceTransform(glm::vec3 translation, glm::vec3 rotation)
{
    for (int i = 0; i < virtualExtrinsics.size(); i++) {
        virtualExtrinsics[i].position = translation;//glm::vec3(translation[0], translation[1], translation[2]);
        virtualExtrinsics[i].rotation = rotation;//glm::vec3(rotation[0], rotation[1], rotation[2]);
    }
}
 
void RenderingParameters::updateSpaceTransform(glm::vec3 translation, glm::vec3 rotation, glm::vec4 fov, int view)
{
    virtualExtrinsics[view].position = translation;// glm::vec3(translation[0], translation[1], translation[2]);
    virtualExtrinsics[view].rotation = rotation;//glm::vec3(rotation[0], rotation[1], rotation[2]);
 
    auto tl = std::tan(fov[0]); //left
    auto tr = std::tan(fov[1]); //right
    auto tt = std::tan(fov[2]); //top
    auto tb = std::tan(fov[3]); //down
 
    auto tw = tr - tl;
    auto th = tt - tb;
 
    auto x0 = (-tl) / tw;
    auto y0 = (-tb) / th;
 
    if (projectionType == InputProvider::ProjectionType::PROJECTION_PERSPECTIVE) {
        std::get<InputProvider::PerspectiveIntrinsics>(virtualIntrasics[view]).focals.x = virtualViewDef[view].x / tw;
        std::get<InputProvider::PerspectiveIntrinsics>(virtualIntrasics[view]).focals.y = virtualViewDef[view].y / th;
 
 
        std::get<InputProvider::PerspectiveIntrinsics>(virtualIntrasics[view]).principle.x = x0 * virtualViewDef[view].x;
        std::get<InputProvider::PerspectiveIntrinsics>(virtualIntrasics[view]).principle.y = (1 - y0) * virtualViewDef[view].y;
    }
    else {
        throw std::runtime_error("Unsupported projection type for virtual parameter"); //TODO support this (Should not be complicated since fov are already the angle that we need for ver_range and hor_range)
    }
}
 
const InputProvider::Extrinsics RenderingParameters::getVirtualExtrinsics(int view)
{
    InputProvider::Extrinsics pose = { .position = {0,0,0}, .rotation = {0,0,0} };
 
    //TODO move that at a better place
    glm::vec3 averagePos = { 0,0,0 };
    for (int i = 0; i < virtualExtrinsics.size(); i++) {
        averagePos += virtualExtrinsics[i].position;
    }
    averagePos /= virtualExtrinsics.size();
 
    if (virtualMode == VirtualMode::Static1) {
#ifdef HVT_UDP_CONTROL
        mutex.lock();
#endif
        pose.position = staticPosition[0] + (virtualExtrinsics[view].position - averagePos );
        pose.rotation = quatToEuler(staticQuaternion[0]);
#ifdef HVT_UDP_CONTROL
        mutex.unlock();
#endif
        //pose.rotation = static[0];
    }
    else if (virtualMode == VirtualMode::Static2) {
#ifdef HVT_UDP_CONTROL
        mutex.lock();
#endif
        pose.position = staticPosition[1] + (virtualExtrinsics[view].position - averagePos );
        pose.rotation = quatToEuler(staticQuaternion[1]);
#ifdef HVT_UDP_CONTROL
        mutex.unlock();
#endif
    }
    else {
        assert(virtualExtrinsics.size() > view);
        pose = virtualExtrinsics[view];
    }
    return pose;
}
 
const InputProvider::Intrinsics RenderingParameters::getVirtualIntrinsics(int view)
{
    assert(virtualIntrasics.size() > view);
    return virtualIntrasics[view];
}
 
void RenderingParameters::setStartingPt(glm::vec3 startPos, glm::vec3 startRot)
{
#ifdef HVT_UDP_CONTROL
    startPosMut.lock();
#endif
    startingPosition = startPos;
    startingRotation = startRot;
#ifdef HVT_UDP_CONTROL
    startPosMut.unlock();
#endif
}
 
const glm::vec3 RenderingParameters::getStartingPosition()
{
#ifdef HVT_UDP_CONTROL
    startPosMut.lock();
#endif
    const glm::vec3 res = glm::vec3(startingPosition);
#ifdef HVT_UDP_CONTROL
    startPosMut.unlock();
#endif
    return res;
}
 
const glm::vec3 RenderingParameters::getStartingRotation()
{
#ifdef HVT_UDP_CONTROL
    startPosMut.lock();
#endif
    const glm::vec3 res = glm::vec3(startingRotation);
#ifdef HVT_UDP_CONTROL
    startPosMut.unlock();
#endif
    return res;
}
 
#ifdef HVT_UDP_CONTROL
void RenderingParameters::setupUDPServer()
{
    
#ifndef WIN32
    struct sockaddr_in client, server;
#endif
    
 
 
#ifdef WIN32
    // Initialize Winsock
    WSADATA wsaData;
    int iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
    if (iResult != 0) {
        printf("WSAStartup failed with error: %d\n", iResult);
        throw std::runtime_error("WSAStartup");
    }
#endif // WIN32
 
 
    if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
    {
#ifdef WIN32
        auto dw = GetLastError();
        LPSTR messageBuffer = nullptr;
 
        //Ask Win32 to give us the string version of that message ID.
        //The parameters we pass in, tell Win32 to create the buffer that holds the message for us (because we don't yet know how long the message string will be).
        size_t size = FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
            NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPSTR)&messageBuffer, 0, NULL);
 
        //Copy the error message into a std::string.
        std::string message(messageBuffer, size);
        WSACleanup();
        throw std::runtime_error("socket error" + message);
        //Free the Win32's string's buffer.
        LocalFree(messageBuffer);
 
        
#else
        tcperror("socket()");
        throw std::runtime_error("socket error");
#endif
    }
 
    server.sin_family = AF_INET;  /* Server is in Internet Domain */
    server.sin_port = htons(portNumber);         /* Use specified port      */
    server.sin_addr.s_addr = INADDR_ANY;/* Server's Internet Address   */
 
    if (bind(s, (struct sockaddr*)&server, sizeof(server)) < 0)
    {
#ifdef WIN32
        WSACleanup();
        throw std::runtime_error("bind error");
#else
        tcperror("bind()");
        throw std::runtime_error("bind error");
#endif
 
    }
 
 
    DWORD tv = 1000;
    if (setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, (char*) &tv, sizeof(tv)) < 0) {
        perror("Error");
    }
 
    /* Find out what port was really assigned and print it */
    namelen = sizeof(server);
    if (getsockname(s, (struct sockaddr*)&server, &namelen) < 0)
    {
#ifdef WIN32
        WSACleanup();
        throw std::runtime_error("error sockName");
#else
        tcperror("getsockname()");
        throw std::runtime_error("error sockName");
#endif
 
    }
 
    PRINT("Port assigned is %d\n", ntohs(server.sin_port));
 
    serverThread = std::thread([this] { this->mainLoop(); });
    
}
void RenderingParameters::mainLoop()
{
    while (serverIsActive) {
        client_address_size = sizeof(client);
 
        if (recvfrom(s, (char*)&udpMsg, sizeof(UDPMessage), 0, (struct sockaddr*)&client,
            &client_address_size) < 0)
        {
#ifdef WIN32
            auto dw = GetLastError();
            LPSTR messageBuffer = nullptr;
            //Ask Win32 to give us the string version of that message ID.
            //The parameters we pass in, tell Win32 to create the buffer that holds the message for us (because we don't yet know how long the message string will be).
            size_t size = FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
                NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPSTR)&messageBuffer, 0, NULL);
            //Copy the error message into a std::string.
            std::string message(messageBuffer, size);
 
            if (!message.empty() > 0) {
                D(PRINT("On port %d message: %s", ntohs(server.sin_port), message.c_str());)
            }
            //
            //throw std::runtime_error("rcv from error" + message);
#else
            tcperror("recvfrom()");
            //throw std::runtime_error("rcv from error");
#endif 
 
        }
        else {
            assert(udpMsg.magic == magicNb);
 
            PRINT("Requested mode: %f", udpMsg.mode);
            PRINT("Received position: %f,%f,%f", udpMsg.viewTransx, udpMsg.viewTransy, udpMsg.viewTransz);
            PRINT("Received rotation: %f,%f,%f,%f", udpMsg.viewRotx, udpMsg.viewRoty, udpMsg.viewRotz, udpMsg.viewRotw);
 
            mutex.lock();
 
            if (udpMsg.mode == 0) {
                virtualMode = VirtualMode::Tracking;
            }
            else if (udpMsg.mode == 1) {
                virtualMode = VirtualMode::Static1;
                staticPosition[0] = { udpMsg.viewTransx, udpMsg.viewTransy, udpMsg.viewTransz };
                staticQuaternion[0] = { udpMsg.viewRotx, udpMsg.viewRoty, udpMsg.viewRotz, udpMsg.viewRotw };
 
            }
            else if (udpMsg.mode == 2) {
                virtualMode = VirtualMode::Static2;
                staticPosition[1] = { udpMsg.viewTransx, udpMsg.viewTransy, udpMsg.viewTransz };
                staticQuaternion[1] = { udpMsg.viewRotx, udpMsg.viewRoty, udpMsg.viewRotz, udpMsg.viewRotw };
            }
            else {
                PRINT("UDP MESSAGE: Invalid mode requested");
            }
 
            mutex.unlock();
 
            PRINT("Camera Activation %f", udpMsg.cameras);
 
            auto actVector = wrapper->getCameraActivation();
 
            //note that it probably will be better to use base 2 here but ok ..
            const auto nbCam = actVector.size();
 
            std::vector<bool> activations;
            bool update = false;
            for (int i = 0; i < nbCam; i++) {
                bool cam = int(udpMsg.cameras / std::pow(10, nbCam - i - 1)) % 2;
                activations.push_back(cam);
                if (cam != actVector[i]) {
                    // the enabling/disabling of camera is not a light operation, it's better to do it only when needed
                    //wrapper->toggleCamera(i);
                    update = true;
                }
            }
 
            if (update) {
                wrapper->setCameraActivation(activations);
            }
 
            PRINT("Value for reset: %f", udpMsg.reset);
 
            if (udpMsg.reset == 1) {
 
                glm::vec3 startP = {udpMsg.initTransx,udpMsg.initTransy,udpMsg.initTransz};
                glm::quat startQ = {udpMsg.initRotx,udpMsg.initRoty, udpMsg.initRotz,udpMsg.initRotw};
 
                PRINT("Received reset position: %f,%f,%f", udpMsg.initTransx, udpMsg.initTransy, udpMsg.initTransz);
                PRINT("Received reset rotation: %f,%f,%f,%f", udpMsg.initRotx, udpMsg.initRoty, udpMsg.initRotz, udpMsg.initRotw);
 
                startPosMut.lock();
 
                startingPosition = startP;
                startingRotation = quatToEuler(startQ); 
 
                startPosMut.unlock();
 
                window->resetOrigin();
 
 
                
            }
        }
 
    }
 
#ifdef WIN32
    WSACleanup();
    closesocket(s);
#else
    close(s);
#endif // 
}
#endif