Simple Node rotation by quaternion

DmitryKonung · Post by **DmitryKonung** » Wed Nov 22, 2017 5:37 am

Please help me!
I need to rotate Node on yaw, pitch, roll angles, and I try to do this by setRotation function, but this method has gimbal lock problem.
I know what my problem can be solved by using quaternions, but I can`t understand HOW.
How can I rotate Node (not shader) by quaternion?

CuteAlien · Post by **CuteAlien** » Wed Nov 22, 2017 10:57 am

You don't really need quaternions, they just can make it easier sometimes.

Following functions might help maybe (no guarantee):

Code: Select all

 
// Given 2 euler rotations find a quaternion describing the relative rotation from the old to the new rotation
irr::core::quaternion getRelativeRotation(const irr::core::vector3df& oldRotationEulerDeg, const irr::core::vector3df& newRotationEulerDeg)
{
    irr::core::quaternion q1(oldRotationEulerDeg*irr::core::DEGTORAD);
    irr::core::quaternion q2(newRotationEulerDeg*irr::core::DEGTORAD);
    irr::core::quaternion qRel(q2*q1.makeInverse());
    return qRel;
}
 
// Given an euler angle + a quaternion with a relative rotation do return an euler angle describing the combined absolute rotation
irr::core::vector3df applyRelativeRotation(const irr::core::vector3df& oldRotationEulerDeg, const irr::core::quaternion& relativeRotation)
{
    // Add relative rotation
    irr::core::quaternion qt(oldRotationEulerDeg*irr::core::DEGTORAD);
    irr::core::quaternion qt2(relativeRotation*qt);
 
    irr::core::vector3df rotateTarget;
    qt2.toEuler(rotateTarget);
    rotateTarget *= irr::core::RADTODEG;
    return rotateTarget;
}

In case you just need a rotation around local axes (adding some yaw, pitch, roll to current rotation) you can maybe use the following function (with useLocalAxes set to true):

Code: Select all

 
irr::core::vector3df rotateAxesXYZToEuler(const irr::core::vector3df& oldRotation, const irr::core::vector3df& rotationAngles, bool useLocalAxes)
{
    irr::core::matrix4 transformation;
    transformation.setRotationDegrees(oldRotation);
    irr::core::vector3df axisX(1,0,0), axisY(0,1,0), axisZ(0,0,1);
    irr::core::matrix4 matRotX, matRotY, matRotZ;
 
    if ( useLocalAxes )
    {
        transformation.rotateVect(axisX);
        transformation.rotateVect(axisY);
        transformation.rotateVect(axisZ);
    }
 
    matRotX.setRotationAxisRadians(rotationAngles.X*irr::core::DEGTORAD, axisX);
    matRotY.setRotationAxisRadians(rotationAngles.Y*irr::core::DEGTORAD, axisY);
    matRotZ.setRotationAxisRadians(rotationAngles.Z*irr::core::DEGTORAD, axisZ);
 
    irr::core::matrix4 newTransform = matRotX * matRotY * matRotZ * transformation;
    return newTransform.getRotationDegrees();
}

DmitryKonung · Post by **DmitryKonung** » Thu Nov 23, 2017 8:10 am

Thanks, CuteAlien.
But, all methods works incorrectly. The Node has wrong orientation. ((
May be I need inverse some axis...

CuteAlien · Post by **CuteAlien** » Thu Nov 23, 2017 11:29 am

Can't help without example where I can see what goes wrong :-)

DmitryKonung · Post by **DmitryKonung** » Thu Nov 23, 2017 12:01 pm

//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
// Given 2 euler rotations find a quaternion describing the relative rotation from the old to the new rotation
irr::core::quaternion getRelativeRotation(const irr::core::vector3df& oldRotationEulerDeg, const irr::core::vector3df& newRotationEulerDeg)
{
irr::core::quaternion q1(oldRotationEulerDeg*irr::core::DEGTORAD);
irr::core::quaternion q2(newRotationEulerDeg*irr::core::DEGTORAD);
irr::core::quaternion qRel(q2*q1.makeInverse());
return qRel;
}

// Given an euler angle + a quaternion with a relative rotation do return an euler angle describing the combined absolute rotation
irr::core::vector3df applyRelativeRotation(const irr::core::vector3df& oldRotationEulerDeg, const irr::core::quaternion& relativeRotation)
{
// Add relative rotation
irr::core::quaternion qt(oldRotationEulerDeg*irr::core::DEGTORAD);
irr::core::quaternion qt2(relativeRotation*qt);

irr::core::vector3df rotateTarget;
qt2.toEuler(rotateTarget);
rotateTarget *= irr::core::RADTODEG;
return rotateTarget;
}
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
irr::core::vector3df rotateAxesXYZToEuler(const irr::core::vector3df& oldRotation, const irr::core::vector3df& rotationAngles, bool useLocalAxes)
{
irr::core::matrix4 transformation;
transformation.setRotationDegrees(oldRotation);
irr::core::vector3df axisX(1, 0, 0), axisY(0, 1, 0), axisZ(0, 0, 1);
irr::core::matrix4 matRotX, matRotY, matRotZ;

if (useLocalAxes)
{
transformation.rotateVect(axisX);
transformation.rotateVect(axisY);
transformation.rotateVect(axisZ);
}

matRotX.setRotationAxisRadians(rotationAngles.X*irr::core::DEGTORAD, axisX);
matRotY.setRotationAxisRadians(rotationAngles.Y*irr::core::DEGTORAD, axisY);
matRotZ.setRotationAxisRadians(rotationAngles.Z*irr::core::DEGTORAD, axisZ);

irr::core::matrix4 newTransform = matRotX * matRotY * matRotZ * transformation;
return newTransform.getRotationDegrees();
}
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
...
/* in main cicle */
{
...
vector3df oldRotationEulerDeg = node_I->getRotation();

vector3df newRotationEulerDeg = vector3df(tet*RADTODEG, 180+psi*RADTODEG, gam*RADTODEG);

//vector3df ROT = rotateAxesXYZToEuler(oldRotationEulerDeg, newRotationEulerDeg, true);

quaternion relativeRotation = getRelativeRotation(oldRotationEulerDeg, newRotationEulerDeg);
vector3df ROT = applyRelativeRotation(oldRotationEulerDeg, relativeRotation);

node_I->setRotation(ROT);

node_I->setPosition(vector3df(X, Y, Z));
node_I->updateAbsolutePosition();
...
}

try to load image on forum

on screenshot red line is the current speed vector.

CuteAlien · Post by **CuteAlien** » Thu Nov 23, 2017 10:20 pm

I didn't mean you should use all 3 functions, just the one (or two) that fit your case :-) I posted 2 solutions which do more or less the same thing.
I made an example with rotateAxesXYZToEuler. See https://bitbucket.org/mzeilfelder/irr-p ... ew-default

Also better to post code others can compile and test. Posting partial code-snippets is nearly never enough. For example in your case I can't tell what tet, psi, gam could be (which range they have, if you maybe sum up values in there or reset them each frame, or maybe even do stranger stuff and have a bug in setting them, etc).
Doesn't mean post your full game. For a good example reduce it to the minimum possible code which still compiles but shows everything that you need to show (my example is also not perfect as it has a few extra lines which I could still kick out...)

edit: I added the other solution (with quaternions) as well now: https://bitbucket.org/mzeilfelder/irr-p ... ew-default
edit2: And you don't need the getRelativeRotation at all - it's just a tool-function which is often useful when you work with quaternions, but in your case you won't need it.

DmitryKonung · Post by **DmitryKonung** » Fri Dec 01, 2017 9:17 am

Here is minimum possible code of my programm:

May be a heve wrong Node axis orientation...

#ifdef _MSC_VER
// We'll also define this to stop MSVC complaining about sprintf().
#define _CRT_SECURE_NO_WARNINGS
#pragma comment(lib, "Irrlicht.lib")
#endif

#define _USE_MATH_DEFINES

#define BUF_SIZE 1000000 //max points
#define MAX_TRACKS 51
#define MAX_INTR_CNT 3

#include <winsock2.h>
#include <windows.h>
#include <irrlicht.h>
#include "driverChoice.h"
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <cmath>
#include <deque>

#include <quaternion.h>

#define Rwgs 6378137.0
#define Rwgs_m 6356752.3142
#define Ewgs 0.0818191908426215
#define alfa_wgs 1/298.257223563

typedef struct {
double Lat;
double Lon;
double H;
double V;
double Psii;
union {
struct {
double gam;
double tet;
} DopNav;
struct {
double a1;
double a2;
} M;
} Un;
float A_AP;
float A_PP;
unsigned int CurrentPM : 8;
unsigned int PointCount : 24;
}TracePoint;

typedef struct {
double x;
double y;
double z;
} ORT_COORD;

extern ORT_COORD **ort_coord;

#pragma comment(lib,"ws2_32.lib") //Winsock Library
//---------------------------------------------------------------------------
#define PORT_3D 888 //The port on which to listen for incoming data
#define pp 50 //
#define KURS 10 // course vector
//---------------------------------------------------------------------------

unsigned int cnt;
unsigned int curent_object = 0;
unsigned int LeftRightObject = 0;

float SCALE_I = 0.004f;
float SCALE = 0.004f;
float SCALE_H = 0.1f;
float SCALE_T = 2.f;

bool force_window_caption = false;

float Psi[MAX_TRACKS];
float Tet[MAX_TRACKS];
float Gam[MAX_TRACKS];

float oldPsi[MAX_TRACKS];
float oldTet[MAX_TRACKS];
float oldGam[MAX_TRACKS];

HANDLE TraceMapping;
LPVOID TracePnt;
//---------------------------------------------------------------------------
unsigned int PathColor[MAX_TRACKS];
//---------------------------------------------------------------------------
double Re_cur;
double Exc_cur;
//---------------------------------------------------------------------------
char TraceFpath[255];
__int64 TraceFsize;

void Trace_PageFileSetPos(int Object);
void GeodesicToOrthogonal(TracePoint *pGData, ORT_COORD *pOData);
ORT_COORD *ort_coord_3d;
TracePoint **trace;

TracePoint ESK_GEO;
ORT_COORD ESK_ORT;

SOCKET s;
struct sockaddr_in server, si_other;
int slen, recv_len;
char buf[4];
WSADATA wsa;

int REP; // current position
int oldREP; // old position

//---------------------------------------------------------------------------
TCHAR szName[] = TEXT("NavTrace");

using namespace irr;
using namespace core;
using namespace scene;
using namespace video;
using namespace io;
using namespace gui;

IrrlichtDevice* device;
scene::ITriangleSelector* selector;
scene::ICameraSceneNode* camera;

vector3df *positionNeed[MAX_TRACKS];

HANDLE hNet_Thread;
DWORD dwNet_Thread;
DWORD WINAPI Net_Thread(LPVOID lpParam);
//------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------
void setActiveCamera(scene::ICameraSceneNode* newActive)
{
if (0 == device)
return;

scene::ICameraSceneNode * active = device->getSceneManager()->getActiveCamera();
active->setInputReceiverEnabled(false);

newActive->setInputReceiverEnabled(true);
device->getSceneManager()->setActiveCamera(newActive);
}
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
// Given 2 euler rotations find a quaternion describing the relative rotation from the old to the new rotation
//irr::core::quaternion getRelativeRotation(const irr::core::vector3df& oldRotationEulerDeg, const irr::core::vector3df& newRotationEulerDeg)
//{
// irr::core::quaternion q1(oldRotationEulerDeg*irr::core::DEGTORAD);
// irr::core::quaternion q2(newRotationEulerDeg*irr::core::DEGTORAD);
// irr::core::quaternion qRel(q2*q1.makeInverse());
// return qRel;
//}

// Given an euler angle + a quaternion with a relative rotation do return an euler angle describing the combined absolute rotation
irr::core::vector3df applyRelativeRotation(const irr::core::vector3df& oldRotationEulerDeg, const irr::core::quaternion& relativeRotation)
{
// Add relative rotation
irr::core::quaternion qt(oldRotationEulerDeg*irr::core::DEGTORAD);
irr::core::quaternion qt2(relativeRotation*qt);

irr::core::vector3df rotateTarget;
qt2.toEuler(rotateTarget);
rotateTarget *= irr::core::RADTODEG;
return rotateTarget;
}
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
irr::core::vector3df rotateAxesXYZToEuler(const irr::core::vector3df& oldRotation, const irr::core::vector3df& rotationAngles, bool useLocalAxes)
{
irr::core::matrix4 transformation;
transformation.setRotationDegrees(oldRotation);
irr::core::vector3df axisX(1, 0, 0), axisY(0, 1, 0), axisZ(0, 0, 1);
irr::core::matrix4 matRotX, matRotY, matRotZ;

if (useLocalAxes)
{
transformation.rotateVect(axisX);
transformation.rotateVect(axisY);
transformation.rotateVect(axisZ);
}

matRotX.setRotationAxisRadians(rotationAngles.X*irr::core::DEGTORAD, axisX);
matRotY.setRotationAxisRadians(rotationAngles.Y*irr::core::DEGTORAD, axisY);
matRotZ.setRotationAxisRadians(rotationAngles.Z*irr::core::DEGTORAD, axisZ);

irr::core::matrix4 newTransform = matRotX * matRotY * matRotZ * transformation;
return newTransform.getRotationDegrees();
}
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
class MyEventReceiver : public IEventReceiver
{
public:
// This is the one method that we have to implement
virtual bool OnEvent(const SEvent& event)
{
// Remember whether each key is down or up
if (event.EventType == irr::EET_KEY_INPUT_EVENT && !event.KeyInput.PressedDown) {

switch (event.KeyInput.Key)
{

default:
break;
}
}

if (event.EventType == irr::EET_KEY_INPUT_EVENT) {
KeyIsDown[event.KeyInput.Key] = event.KeyInput.PressedDown;
}

if (event.EventType == EET_KEY_INPUT_EVENT && event.KeyInput.PressedDown == false)
{
if (OnKeyUp(event.KeyInput.Key))
return true;
}

return false;
}

bool OnKeyUp(irr::EKEY_CODE keyCode) {
return true;
}

// This is used to check whether a key is being held down
virtual bool IsKeyDown(EKEY_CODE keyCode) const
{
return KeyIsDown[keyCode];
}

MyEventReceiver(/*scene::ISceneNode* terrain,/* scene::ISceneNode* skybox,*/ scene::ISceneNode* skydome) :
/*Terrain(terrain),/* Skybox(skybox),*/ Skydome(skydome), showBox(true), showDebug(false)
{
//Skybox->setVisible(showBox);
Skydome->setVisible(!showBox);

for (u32 i = 0; i<KEY_KEY_CODES_COUNT; ++i)
KeyIsDown = false;
}

private:

//scene::ISceneNode* Terrain;
//scene::ISceneNode* Skybox;
scene::ISceneNode* Skydome;
bool showBox;
bool showDebug;

// We use this array to store the current state of each key
bool KeyIsDown[KEY_KEY_CODES_COUNT];
};
//###################################################################
//###################################################################
//###################################################################
int main()
{
// Общая часть ===================================================================
Re_cur = Rwgs;
Exc_cur = Ewgs;

TraceFsize = sizeof(TracePoint)*BUF_SIZE*MAX_TRACKS;
trace = new TracePoint*[MAX_TRACKS];

ort_coord_3d = new ORT_COORD[MAX_TRACKS];

for (int i = 0; i < MAX_TRACKS; i++) {
positionNeed = new vector3df[BUF_SIZE];
memset(positionNeed, 0, sizeof(vector3df)* BUF_SIZE);
}

char chApPath[MAX_PATH];
LPTSTR lpApPath = chApPath;
GetModuleFileName(NULL, lpApPath, MAX_PATH);
*strrchr(lpApPath, '\\') = 0;
printf("%s\n\r", lpApPath);
SetCurrentDirectory(lpApPath);

// SOCKETS =================================================================
slen = sizeof(si_other);

//Initialise winsock
printf("\nInitialising Winsock...");
if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
{
printf("Failed. Error Code : %d", WSAGetLastError());
exit(EXIT_FAILURE);
}
printf("Initialised.\n");

//Create a socket
if ((s = socket(AF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET)
{
printf("Could not create socket : %d", WSAGetLastError());
}
printf("Socket created.\n");

//Prepare the sockaddr_in structure
server.sin_family = AF_INET;
server.sin_addr.s_addr = INADDR_ANY;
server.sin_port = htons(PORT_3D);

//Bind
if (bind(s, (struct sockaddr *)&server, sizeof(server)) == SOCKET_ERROR)
{
printf("Bind failed with error code : %d", WSAGetLastError());
getchar();
exit(EXIT_FAILURE);
}
puts("Bind done");

printf("SOCKET : %d\n", (int)s);

// 3D =====================================================================
video::E_DRIVER_TYPE driverType = video::EDT_OPENGL;
if (driverType == video::EDT_COUNT)
return 1;

irr::SIrrlichtCreationParameters params;

params.DriverType = driverType;
params.WindowSize = core::dimension2d<u32>(1600, 1200);
params.Bits = 32;
//params.UsePerformanceTimer = true;
params.Vsync = true;
//params.AntiAlias = 8;

IrrlichtDevice* device = createDeviceEx(params);

// create device
if (device == 0)
return 1; // could not create selected driver.

device->setResizable(true);
video::IVideoDriver* driver = device->getVideoDriver();
scene::ISceneManager* smgr = device->getSceneManager();
gui::IGUIEnvironment* env = device->getGUIEnvironment();

driver->setTextureCreationFlag(video::ETCF_ALWAYS_32_BIT, true);
camera = smgr->addCameraSceneNodeMaya(NULL, 50.0f, 0.2f);
camera->setFarValue(1e+6);

device->getCursorControl()->setVisible(true);

IGUIEnvironment* guienv = device->getGUIEnvironment();
IGUISkin* skin = guienv->getSkin();
IGUIFont* font = guienv->getFont("myfont.xml");
if (font) skin->setFont(font);
//```````````````````````````````````````````````````
TraceMapping = OpenFileMapping(FILE_MAP_ALL_ACCESS, false, szName);
if (TraceMapping == NULL) {
printf("Error while opening file");
return 1;
}
cnt = 0;
Trace_PageFileSetPos(0);
//```````````````````````````````````````````````````
scene::ISceneNode* skybox = smgr->addSkyBoxSceneNode(
driver->getTexture("irrlicht4_up.jpg"),
driver->getTexture("irrlicht4_dn.jpg"),
driver->getTexture("irrlicht4_lf.jpg"),
driver->getTexture("irrlicht4_rt.jpg"),
driver->getTexture("irrlicht4_ft.jpg"),
driver->getTexture("irrlicht4_bk.jpg"));

scene::ISceneNode* skydome = smgr->addSkyDomeSceneNode(driver->getTexture("skydome.jpg"), 16, 8, 0.95f, 2.0f);

driver->setTextureCreationFlag(video::ETCF_CREATE_MIP_MAPS, true);
//create event receiver
MyEventReceiver receiver(/*terrain, skybox,*/ skydome);
device->setEventReceiver(&receiver);
//```````````````````````````````````````````````````
IMesh *mesh_I = smgr->getMesh("t50.3ds");

scene::ISceneNode * node_I[MAX_INTR_CNT];
for (int i = 0; i < MAX_INTR_CNT; i++) {
node_I = smgr->addMeshSceneNode(mesh_I, 0, -1, core::vector3df(0, 0, 0), core::vector3df(45, 0, 0));
node_I->setScale(core::vector3df(SCALE_I, SCALE_I, SCALE_I));
}

video::ITexture* texture[MAX_TRACKS];
for (int i = 0; i < MAX_INTR_CNT; i++) {
if (node_I) {
node_I->setMaterialFlag(video::EMF_LIGHTING, true);
node_I->setVisible(false);
node_I->setMaterialFlag(video::EMF_NORMALIZE_NORMALS, true);
node_I->setMaterialFlag(video::EMF_ANTI_ALIASING, true);
node_I[i]->setMaterialFlag(video::EMF_ANISOTROPIC_FILTER, true);
smgr->addLightSceneNode(node_I[i], core::vector3df(1, 1, 1), video::SColorf(1.0f, 1.0f, 1.0f), 10);
}
}

smgr->setAmbientLight(video::SColorf(0.6f, 0.6f, 0.6f));
//---------------------------------------------------------------------------------
int lastFPS = -1;
//---------------------------------------------------------------------------------
u32 then = device->getTimer()->getTime();

// This is the movemen speed in units per second.
const f32 MOVEMENT_SPEED = 5.f;

cnt = 0;

Trace_PageFileSetPos(0);
memcpy(&ESK_GEO, &trace[0][0], sizeof(TracePoint));
GeodesicToOrthogonal(&ESK_GEO, &ESK_ORT);
printf("ESK.x = %f ; ESK.y = %f ; ESK.z = %f\n", ESK_ORT.x, ESK_ORT.y, ESK_ORT.z);

for (int i = 0; i<MAX_TRACKS; i++) {
Trace_PageFileSetPos(i);
if (trace[i][0].Lat != 0 && trace[i][0].Lon != 0)
for (int j = 0; j<trace[i][0].PointCount; j++) {

// Converting all coordinates to ЕСК ()
GeodesicToOrthogonal(&trace[i][j], &ort_coord_3d[i]);

ORT_COORD R_prom;
R_prom.x = ort_coord_3d[i].x - ESK_ORT.x;
R_prom.y = ort_coord_3d[i].y - ESK_ORT.y;
R_prom.z = ort_coord_3d[i].z - ESK_ORT.z;
ORT_COORD R_ESK;
R_ESK.x = R_prom.x * (-1.*cos(ESK_GEO.Lon)*sin(ESK_GEO.Lat)) +
R_prom.y * (-1.*sin(ESK_GEO.Lon)*sin(ESK_GEO.Lat)) +
R_prom.z * cos(ESK_GEO.Lat);

R_ESK.y = R_prom.x * (cos(ESK_GEO.Lon)*cos(ESK_GEO.Lat)) +
R_prom.y * (sin(ESK_GEO.Lon)*cos(ESK_GEO.Lat)) +
R_prom.z * sin(ESK_GEO.Lat);

R_ESK.z = R_prom.x * (-1.*sin(ESK_GEO.Lon)) +
R_prom.y * cos(ESK_GEO.Lon);

positionNeed[i][j].Z = R_ESK.x * 0.01;
positionNeed[i][j].Y = R_ESK.y * 0.01;
positionNeed[i][j].X = R_ESK.z * 0.01;
}
}
//---------------------------------------------------------------------------
REP = 0;

hNet_Thread = CreateThread(NULL, 0, Net_Thread, 0, 0, &dwNet_Thread);

while (device->run())
{
// 3D =================
// Work out a frame delta time.
const u32 now = device->getTimer()->getTime();
const f32 frameDeltaTime = (f32)(now - then) / 1000.f; // Time in seconds
then = now;

static float Zoom = 1.5f;
if (receiver.IsKeyDown(irr::KEY_KEY_Z))
Zoom += MOVEMENT_SPEED / 10.f * frameDeltaTime;
else if (receiver.IsKeyDown(irr::KEY_KEY_Q))
Zoom -= MOVEMENT_SPEED / 10.f * frameDeltaTime;
camera->setFOV(Zoom);

//``````````````````````````````````````````````````````
if (REP >= pp) {
for (int i = 0; i < MAX_TRACKS; i++) {
Trace_PageFileSetPos(i);
if (trace[i][0].Lat != 0 && trace[i][0].Lon != 0) {
unsigned int red = (PathColor[i] & 0x00FF0000) >> 16;
unsigned int green = (PathColor[i] & 0x0000FF00) >> 8;
unsigned int blue = (PathColor[i] & 0x000000FF);

if (i < MAX_INTR_CNT) {
if (node_I[i]) {
dimension2d<u32> image_size = dimension2d<u32>(2, 2);
IImage* image = driver->createImage(ECF_R8G8B8, image_size);
ECOLOR_FORMAT color_format = image->getColorFormat();
for (int J = 0; J < image_size.Height; J++)
for (int I = 0; I < image_size.Width; I++)
image->setPixel(J, I, SColor(255, blue, green, red), false);
ITexture *color = driver->addTexture("color", image);
node_I[i]->setMaterialTexture(0, color);

node_I[i]->setVisible(true);
node_I[i]->setScale(vector3df(SCALE_I, SCALE_I, SCALE_I));
//``````````````````````````````````````````````````````````````````
//pitch, yaw, roll = tet, psi, gam
//yaw, pitch, roll = psi, tet, gam
core::vector3df oldRotation = node_I[i]->getRotation();
irr::core::vector3df rotationAngles(0, 0, 0);

Psi[i] = trace[i][REP].Psii*irr::core::RADTODEG;
Tet[i] = trace[i][REP].Un.DopNav.tet*irr::core::RADTODEG;
Gam[i] = trace[i][REP].Un.DopNav.gam*irr::core::RADTODEG;

oldPsi[i] = trace[i][oldREP].Psii*irr::core::RADTODEG;
oldTet[i] = trace[i][oldREP].Un.DopNav.tet*irr::core::RADTODEG;
oldGam[i] = trace[i][oldREP].Un.DopNav.gam*irr::core::RADTODEG;

rotationAngles.X = Psi[i] - oldPsi[i];
rotationAngles.Y = Tet[i] - oldTet[i];
rotationAngles.Z = Gam[i] - oldGam[i];

if (!rotationAngles.equals(irr::core::vector3df(0, 0, 0)))
{
// without quaternions
core::vector3df newRot = rotateAxesXYZToEuler(oldRotation, rotationAngles, true);
// with quaternions
//irr::core::quaternion qt(rotationAngles*irr::core::DEGTORAD);
//core::vector3df newRot = applyRelativeRotation(oldRotation, qt);

node_I[i]->setRotation(newRot);
}

//``````````````````````````````````````````````````````````````````
node_I[i]->setPosition(vector3df(positionNeed[i][REP].X, positionNeed[i][REP].Y, positionNeed[i][REP].Z));
node_I[i]->updateAbsolutePosition();
}
}
}
}

oldREP = REP;
} else {
for (int i = 0; i < MAX_TRACKS; i++)
if (i < MAX_INTR_CNT)
if (node_I[i])
node_I[i]->setRotation(vector3df(0,0,0));
}
driver->beginScene(true, true, video::SColor(255, 111, 111, 111));
smgr->drawAll(); // draw the 3d scene
env->drawAll();
//---------------------------------------------------------------------------------------
gui::IGUIFont* font2 = device->getGUIEnvironment()->getFont("Rus_Font.xml");
char ach[255] = "";

static float SumPsi = 0;
SumPsi += (oldPsi[0] - Psi[0]);
static float SumTet = 0;
SumTet += (oldTet[0] - Tet[0]);
static float SumGam = 0;
SumGam += (oldGam[0] - Gam[0]);

sprintf(ach, "dPsi %3.7f %3.7f %3.7f ----- %3.7f", oldPsi[0] - Psi[0], oldPsi[1] - Psi[1], oldPsi[2] - Psi[2], SumPsi);
font2->draw(ach, core::rect<s32>(500, 20, 200, 20), video::SColor(150, 0, 0, 0));

sprintf(ach, "dTet %3.7f %3.7f %3.7f ----- %3.7f", oldTet[0] - Tet[0], oldTet[1] - Tet[1], oldTet[2] - Tet[2], SumTet);
font2->draw(ach, core::rect<s32>(500, 40, 200, 20), video::SColor(150, 0, 0, 0));

sprintf(ach, "dGam %3.7f %3.7f %3.7f ----- %3.7f", oldGam[0] - Gam[0], oldGam[1] - Gam[1], oldGam[2] - Gam[2], SumGam);
font2->draw(ach, core::rect<s32>(500, 60, 200, 20), video::SColor(150, 0, 0, 0));

//---------------------------------------------------------------------------------------
SMaterial m;
m.Lighting = false;
driver->setMaterial(m);
driver->setTransform(video::ETS_WORLD, core::matrix4());

SMaterial mat;
mat.AntiAliasing = true;
mat.Lighting = false;
for (int i = 0; i<MAX_TRACKS; i++) {
if (i < MAX_INTR_CNT) {
Trace_PageFileSetPos(i);
if (trace[i][0].Lat != 0 && trace[i][0].Lon != 0) {
mat.Thickness = 3;
driver->setMaterial(mat);
unsigned int red = (PathColor[i] & 0x00FF0000) >> 16;
unsigned int green = (PathColor[i] & 0x0000FF00) >> 8;
unsigned int blue = (PathColor[i] & 0x000000FF);
for (int cnt = pp; cnt < trace[i][0].PointCount; cnt += pp) {

if ((fabs(positionNeed[i][cnt - pp].X - positionNeed[i][cnt].X) < 1.0) &&
(fabs(positionNeed[i][cnt - pp].Y - positionNeed[i][cnt].Y) < 1.0) &&
(fabs(positionNeed[i][cnt - pp].Z - positionNeed[i][cnt].Z) < 1.0))
driver->draw3DLine(vector3df(positionNeed[i][cnt - pp].X,
positionNeed[i][cnt - pp].Y,
positionNeed[i][cnt - pp].Z),
vector3df(positionNeed[i][cnt].X,
positionNeed[i][cnt].Y,
positionNeed[i][cnt].Z),
SColor(255, blue, green, red)
);
}
//```````````````````````````

if (REP >= pp) {
mat.Thickness = 80;
driver->setMaterial(mat);
driver->draw3DLine(vector3df(positionNeed[i][REP - pp].X,
positionNeed[i][REP - pp].Y,
positionNeed[i][REP - pp].Z),
vector3df(positionNeed[i][REP].X,
positionNeed[i][REP].Y,
positionNeed[i][REP].Z),
SColor(255, 255, 0, 0));

mat.Thickness = 5;
driver->setMaterial(mat);
double PSI = 90.f*DEGTORAD - trace[i][REP].Psii;
double TET = trace[i][REP].Un.DopNav.tet;
driver->draw3DLine(vector3df(positionNeed[i][REP].X,
positionNeed[i][REP].Y,
positionNeed[i][REP].Z),
vector3df((positionNeed[i][REP].X + KURS*cos(TET)*cos(PSI)),
(positionNeed[i][REP].Y + KURS*sin(TET)),
(positionNeed[i][REP].Z + KURS*cos(TET)*sin(PSI))),
SColor(255, 255, 0, 0));
}
}

}
}
// device->getGUIEnvironment()->drawAll(); // draw the gui environment (the logo)
driver->endScene();
//``````````````````````````````````````````````````````
if (receiver.IsKeyDown(irr::KEY_SPACE)) {
camera->setPosition(core::vector3df(100, 100, 100));
camera->setTarget(core::vector3df(0, 0, 0));
camera->updateAbsolutePosition();
setActiveCamera(camera);
curent_object = 0;
force_window_caption = true;
}

else if (receiver.IsKeyDown(irr::KEY_LEFT)) {
if (LeftRightObject != irr::KEY_LEFT) {
if (curent_object > 0) {
curent_object--;
Trace_PageFileSetPos(curent_object);
if (trace[curent_object][0].PointCount) {
camera->setPosition(core::vector3df(
positionNeed[curent_object][0].X,
positionNeed[curent_object][0].Y,
positionNeed[curent_object][0].Z));
camera->setTarget(core::vector3df(
positionNeed[curent_object][0].X,
positionNeed[curent_object][0].Y,
positionNeed[curent_object][0].Z));
setActiveCamera(camera);
}
}
LeftRightObject = irr::KEY_LEFT;
force_window_caption = true;
}
}

else if (receiver.IsKeyDown(irr::KEY_RIGHT)) {
if (LeftRightObject != irr::KEY_RIGHT) {
if (curent_object < MAX_TRACKS - 1) {
curent_object++;
Trace_PageFileSetPos(curent_object);
if (trace[curent_object][0].PointCount) {
camera->setPosition(core::vector3df(
positionNeed[curent_object][0].X,
positionNeed[curent_object][0].Y,
positionNeed[curent_object][0].Z));
camera->setTarget(core::vector3df(
positionNeed[curent_object][0].X,
positionNeed[curent_object][0].Y,
positionNeed[curent_object][0].Z));
setActiveCamera(camera);
}
}
LeftRightObject = irr::KEY_RIGHT;
force_window_caption = true;
}
}
//`````````````````````````````````
int fps = driver->getFPS();

if (lastFPS != fps || force_window_caption)
{
core::stringw tmp(L"Navilight 3D track module - [");
tmp += driver->getName();
tmp += L"] fps: ";
tmp += fps;
tmp += L" object# ";
tmp += curent_object;

device->setWindowCaption(tmp.c_str());
lastFPS = fps;
force_window_caption = false;
}

//REP+=pp;
} // end of while
//```````````````````
device->drop();

closesocket(s);
WSACleanup();

return 0;
}
//###################################################################
//###################################################################
//###################################################################
// ---------------------------------------------------------------------------
void Trace_PageFileSetPos(int Object) {

if (TracePnt != NULL) {
if (!UnmapViewOfFile(TracePnt)) {
Beep(200, 1000);
printf("Error while unmapping \n");
return;
}
TracePnt = NULL;
}

TracePnt = MapViewOfFile(TraceMapping, FILE_MAP_READ | FILE_MAP_WRITE, 0, (DWORD)(Object * sizeof(TracePoint)*BUF_SIZE), sizeof(TracePoint)*BUF_SIZE);
if (TracePnt == NULL) {
printf("Error while mapping \n");
return;
}
trace[Object] = (TracePoint*)(TracePnt);
}
// ---------------------------------------------------------------------------
void GeodesicToOrthogonal(TracePoint *pGData, ORT_COORD *pOData)
{
double C_lat = cos(pGData->Lat);
double T_lat = tan(pGData->Lat);
double OverSQRT = 1. + (1. - Exc_cur*Exc_cur)*T_lat*T_lat;
if (OverSQRT > 0) {
double SQ_RT = sqrt(OverSQRT);
pOData->x = (Re_cur / SQ_RT + pGData->H*C_lat) *cos(pGData->Lon);
pOData->y = (Re_cur / SQ_RT + pGData->H*C_lat) *sin(pGData->Lon);
pOData->z = Re_cur*(1. - Exc_cur*Exc_cur)*tan(pGData->Lat) / SQ_RT + pGData->H*sin(pGData->Lat);
}
}
// --------------------------------------------------------------------------
DWORD WINAPI Net_Thread(LPVOID lpParam) {

union {
char sendbuf[sizeof(int)+sizeof(int)*MAX_TRACKS];
struct {
unsigned int position;
unsigned int color[MAX_TRACKS];
unsigned int ObjType[MAX_TRACKS - MAX_INTR_CNT];
} data;
} SocketNVL;

while (1) {
memset(&SocketNVL, 0, sizeof(SocketNVL));
//try to receive some data, this is a blocking call
if ((recv_len = recvfrom(s, SocketNVL.sendbuf, sizeof(SocketNVL), 0, (struct sockaddr *) &si_other, &slen)) == SOCKET_ERROR)
{
printf("recvfrom() failed with error code : %d", WSAGetLastError());
exit(EXIT_FAILURE);
}

int TREP = 0;
TREP = SocketNVL.data.position;
memcpy(PathColor, &SocketNVL.data.color, sizeof(PathColor));
//memcpy(ObjType, SocketNVL.data.ObjType, sizeof(ObjType));

if (TREP >= BUF_SIZE) REP = BUF_SIZE;
else REP = TREP;

printf("%d\n\r", REP);
}
}

CuteAlien · Post by **CuteAlien** » Fri Dec 01, 2017 11:24 am

OK, too much code this time to understand quickly (in the lunch-pause...). But on first view I see some suspicious code where you calculate rotation based on some difference between new and old values. It's suspicious because the rotation happening is around current local axes. And those are different each frame (as the local axes change with each rotation). So any calculation involving both old and new rotation would have to transform those rotations first. It kinda looks like you still try to have some absolute pitch/yaw/roll which is summed up. But you can't have that without getting into gimbal lock. You have to start with pitch/yaw/roll each frame new from 0 - as it only makes sense to add the changes since last frame to the current local axes. The meaning of pitch/yaw/roll does change each frame (well, each frame where the rotation axes have changed) as it always is around the current local axes - so don't try summing those values or having some difference of those values over several frames - you are comparing apples with oranges otherwise.

(and if you don't do that - sorry, running out of time or I don't get to eat right now - might take a look again on weekend if I find time)

edit: I took another look - but that's too much code to understand. And it involves a lot of data which is not part of the post - so I still have no idea what kind of data you are working with here.

DmitryKonung · Post by **DmitryKonung** » Tue Dec 05, 2017 1:37 pm

Thank you, CuteAlien. Sorry, it`s hard to understand me, I ‘am junior in 3d. And I very grateful for your help!
on every frame I have new absolutely values of yaw, pitch and roll. For example: for Yaw 40.1->40.2->40.3 degrees. And now I need to rotate my node on this angles (in absolutely coordinates). I think what a need to recalculate angles in local coordinates (to local axis) to make rotation, but I how can I do that…

CuteAlien · Post by **CuteAlien** » Tue Dec 05, 2017 2:09 pm

The problem is I don't know what absolute values of yaw, pitch, roll would mean. You can have rotation around some non-changing axis-system (like x,y,z axis) and you can sum that up. That's euler angles (which can run into gimbal lock if you try to work with them by just increasing the values in small steps). Pitch-yaw-roll should always be about the local coordinate system.
Well, absolute values of pitch-yaw-roll would probably be controller movement, thought it makes usually no sense summing that up except for statistical reasons.

But... I'm wondering by now if my functions are always correct. Because I just remember in my test-cases (code is from some editor) I always only rotated around one axes - never around all 3 at the same time. So might be an untested case. You could try if you get different results if you rotate them one-by-one (call the functions 3 times by passing x,y,z in series). And another reason things could go wrong in your case is if the order in your original data is given different. Here the order is around x - then y - then z. You get different results with another order. So maybe your original data had a different order.

Irrlicht Engine

Simple Node rotation by quaternion

Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion

Re: Simple Node rotation by quaternion